/*=========================================================================
 Authors: Michael Kazhdan and Matthew Bolitho
 at Johns Hopkins University, 2006-10

 Copyright (c) 2006-10, Michael Kazhdan and Matthew Bolitho,
 Johns Hopkins University.
 All rights reserved.

 Redistribution and use in source and binary forms, with or without
 modification, are permitted provided that the following conditions are met:

 Redistributions of source code must retain the above copyright notice,
 this list of conditions and the following disclaimer.
 Redistributions in binary form must reproduce the above copyright notice,
 this list of conditions and the following disclaimer in the documentation
 and/or other materials provided with the distribution.
 Neither the name of the Johns Hopkins University nor the names of its
 contributors may be used to endorse or promote products derived from this
 software without specific prior written permission.

 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
 BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
 OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
 OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
 OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
 ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
=========================================================================*/


#if defined(_MSC_VER)
# pragma warning (push)
# pragma warning (disable: 4267) /* conversion from 'size_t' to 'int', possible loss of dataconversion from 'size_t' to 'int', possible loss of data */
# pragma warning (disable: 4701) /* potentially uninitialized local variable */
#endif

#include <vtkSmartPointer.h>
#include <vtkXMLPolyDataReader.h>
#include <vtkPolyData.h>
#include <vtkPointData.h>
#include <vtkFloatArray.h>

#include "MultiGridOctest.h"

#define ITERATION_POWER 1.0/3
#define MEMORY_ALLOCATOR_BLOCK_SIZE 1<<12

#define READ_SIZE 1024

#define PAD_SIZE (Real(1.0))

const Real EPSILON=Real(1e-6);
const Real ROUND_EPS=Real(1e-5);


/////////////////////
// SortedTreeNodes //
/////////////////////
inline SortedTreeNodes::SortedTreeNodes(void)
{
  nodeCount=NULL;
  treeNodes=NULL;
  maxDepth=0;
}

inline SortedTreeNodes::~SortedTreeNodes(void)
{
  if(nodeCount){delete[] nodeCount;}
  if(treeNodes){delete[] treeNodes;}
  nodeCount=NULL;
  treeNodes=NULL;
}

inline void SortedTreeNodes::set(TreeOctNode& root,const int& setIndex)
{
  if(nodeCount)
  {delete[] nodeCount;}

  if(treeNodes)
  {delete[] treeNodes;}

  maxDepth=root.maxDepth()+1;
  nodeCount=new int[maxDepth+1];
  treeNodes=new TreeOctNode*[root.nodes()];

  TreeOctNode* temp=root.nextNode();
  int i,cnt=0;
  while(temp)
  {
    treeNodes[cnt++]=temp;
    temp=root.nextNode(temp);
  }
  qsort(treeNodes,cnt,sizeof(const TreeOctNode*),TreeOctNode::CompareForwardPointerDepths);

  for(i=0;i<=maxDepth;i++)
  {nodeCount[i]=0;}

  for(i=0;i<cnt;i++)
  {
    if(setIndex)
    {treeNodes[i]->nodeData.nodeIndex=i;}
    nodeCount[treeNodes[i]->depth()+1]++;
  }

  for(i=1;i<=maxDepth;i++)
  {nodeCount[i]+=nodeCount[i-1];}
}


//////////////////
// TreeNodeData //
//////////////////
//int TreeNodeData::UseIndex=1;

inline TreeNodeData::TreeNodeData()
{

  if(UseIndex){
    nodeIndex=-1;
    centerWeightContribution=0;
  }
  else{mcIndex=0;}
  value=0;
}

inline TreeNodeData::~TreeNodeData()
{}


////////////
// Octree //
////////////
//template<int Degree>
//double Octree<Degree>::maxMemoryUsage=0;

/*
template<int Degree>
inline double Octree<Degree>::MemoryUsage(void)
{
	double mem=MemoryInfo::Usage()/(1<<20);
	if(mem>maxMemoryUsage)
    {maxMemoryUsage=mem;}
	return mem;
}
*/

  template<int Degree>
  inline Octree<Degree>::Octree(void)
{
  radius=0;
  width=0;
  postNormalSmooth=0;
}

template<int Degree>
inline void Octree<Degree>::setNodeIndices(TreeOctNode& node,int& idx)
{
  node.nodeData.nodeIndex=idx;
  idx++;
  if(node.children)
  {for(int i=0;i<Cube::CORNERS;i++)
    {setNodeIndices(node.children[i],idx);
    }
  }
}

template<int Degree>
inline int Octree<Degree>::NonLinearSplatOrientedPoint(TreeOctNode* node,const Point3D<Real>& position,const Point3D<Real>& normal)
{
  double x,dxdy,dxdydz,dx[DIMENSION][3];
  int i,j,k;
  TreeOctNode::Neighbors& neighbors=neighborKey.setNeighbors(node);
  double twidth;
  Point3D<Real> center;
  Real w;

  node->centerAndWidth(center,w);
  twidth=w;
  for(i=0;i<3;i++)
  {
    x=(center.coords[i]-position.coords[i]-twidth)/twidth;
    dx[i][0]=1.125+1.500*x+0.500*x*x;
    x=(center.coords[i]-position.coords[i])/twidth;
    dx[i][1]=0.750        -      x*x;
    dx[i][2]=1.0-dx[i][1]-dx[i][0];
  }
  for(i=0;i<3;i++)
  {
    for(j=0;j<3;j++)
    {
      dxdy=dx[0][i]*dx[1][j];
      for(k=0;k<3;k++)
      {
        if(neighbors.neighbors[i][j][k])
        {
          dxdydz=dxdy*dx[2][k];
          int idx=neighbors.neighbors[i][j][k]->nodeData.nodeIndex;
          if(idx<0)
          {
            Point3D<Real> n;
            n.coords[0]=n.coords[1]=n.coords[2]=0;
            idx=neighbors.neighbors[i][j][k]->nodeData.nodeIndex=int(normals->size());
            normals->push_back(n);
          }
          (*normals)[idx].coords[0]+=Real(normal.coords[0]*dxdydz);
          (*normals)[idx].coords[1]+=Real(normal.coords[1]*dxdydz);
          (*normals)[idx].coords[2]+=Real(normal.coords[2]*dxdydz);
        }
      }
    }
  }
  return 0;
}

template<int Degree>
inline void Octree<Degree>::NonLinearSplatOrientedPoint(const Point3D<Real>& position,const Point3D<Real>& normal,const int& splatDepth,const Real& samplesPerNode,
                                                        const int& minDepth,const int& maxDepth)
{
  double dx;
  Point3D<Real> n;
  TreeOctNode* temp;
  int i;
  double twidth;
  Point3D<Real> myCenter;
  Real myWidth;
  myCenter.coords[0]=myCenter.coords[1]=myCenter.coords[2]=Real(0.5);
  myWidth=Real(1.0);

  temp=&tree;
  while(temp->depth()<splatDepth)
  {
    if(!temp->children)
    {
      printf("Octree<Degree>::NonLinearSplatOrientedPoint error\n");
      return;
    }
    int cIndex=TreeOctNode::CornerIndex(myCenter,position);
    temp=&temp->children[cIndex];
    myWidth/=2;
    if(cIndex&1){myCenter.coords[0]+=myWidth/2;}
    else		{myCenter.coords[0]-=myWidth/2;}
    if(cIndex&2){myCenter.coords[1]+=myWidth/2;}
    else		{myCenter.coords[1]-=myWidth/2;}
    if(cIndex&4){myCenter.coords[2]+=myWidth/2;}
    else		{myCenter.coords[2]-=myWidth/2;}
  }
  Real alpha,newDepth;
  NonLinearGetSampleDepthAndWeight(temp,position,samplesPerNode,newDepth,alpha);

  if(newDepth<minDepth)
  {newDepth=Real(minDepth);}

  if(newDepth>maxDepth)
  {newDepth=Real(maxDepth);}

  int topDepth=int(ceil(newDepth));

  dx=1.0-(topDepth-newDepth);
  if(topDepth<=minDepth)
  {
    topDepth=minDepth;
    dx=1;
  }
  else if(topDepth>maxDepth)
  {
    topDepth=maxDepth;
    dx=1;
  }
  while(temp->depth()>topDepth)
  {temp=temp->parent;}

  while(temp->depth()<topDepth)
  {
    if(!temp->children)
    {temp->initChildren();}
    int cIndex=TreeOctNode::CornerIndex(myCenter,position);
    temp=&temp->children[cIndex];
    myWidth/=2;
    if(cIndex&1){myCenter.coords[0]+=myWidth/2;}
    else		{myCenter.coords[0]-=myWidth/2;}
    if(cIndex&2){myCenter.coords[1]+=myWidth/2;}
    else		{myCenter.coords[1]-=myWidth/2;}
    if(cIndex&4){myCenter.coords[2]+=myWidth/2;}
    else		{myCenter.coords[2]-=myWidth/2;}
  }
  twidth=1.0/(1<<temp->depth());
  for(i=0;i<DIMENSION;i++)
  {n.coords[i]=normal.coords[i]*alpha/Real(pow(twidth,3))*Real(dx);}

  NonLinearSplatOrientedPoint(temp,position,n);
  if(fabs(1.0-dx)>EPSILON)
  {
    dx=Real(1.0-dx);
    temp=temp->parent;
    twidth=1.0/(1<<temp->depth());

    for(i=0;i<DIMENSION;i++)
    {n.coords[i]=normal.coords[i]*alpha/Real(pow(twidth,3))*Real(dx);}

    NonLinearSplatOrientedPoint(temp,position,n);
  }
}

template<int Degree>
inline void Octree<Degree>::NonLinearGetSampleDepthAndWeight(TreeOctNode* node,const Point3D<Real>& position,const Real& samplesPerNode,Real& depth,Real& weight)
{
  TreeOctNode* temp=node;
  weight=Real(1.0)/NonLinearGetSampleWeight(temp,position);
  if(weight>=samplesPerNode+1)
  {depth=Real(temp->depth()+std::log(weight/(samplesPerNode+1))/std::log(double(1<<(DIMENSION-1))));}
  else
  {
    Real oldAlpha,newAlpha;
    oldAlpha=newAlpha=weight;
    while(newAlpha<(samplesPerNode+1) && temp->parent)
    {
      temp=temp->parent;
      oldAlpha=newAlpha;
      newAlpha=Real(1.0)/NonLinearGetSampleWeight(temp,position);
    }
    depth=Real(temp->depth()+std::log(newAlpha/(samplesPerNode+1))/std::log(newAlpha/oldAlpha));
  }
  weight=Real(pow(double(1<<(DIMENSION-1)),-double(depth)));
}

template<int Degree>
inline Real Octree<Degree>::NonLinearGetSampleWeight(TreeOctNode* node,const Point3D<Real>& position)
{
  Real weight=0;
  double x,dxdy,dx[DIMENSION][3];
  int i,j,k;
  TreeOctNode::Neighbors& neighbors=neighborKey.setNeighbors(node);
  double twidth;
  Point3D<Real> center;
  Real w;
  node->centerAndWidth(center,w);
  twidth=w;

  for(i=0;i<DIMENSION;i++)
  {
    x=(center.coords[i]-position.coords[i]-twidth)/twidth;
    dx[i][0]=1.125+1.500*x+0.500*x*x;
    x=(center.coords[i]-position.coords[i])/twidth;
    dx[i][1]=0.750        -      x*x;
    dx[i][2]=1.0-dx[i][1]-dx[i][0];
  }

  for(i=0;i<3;i++)
  {
    for(j=0;j<3;j++)
    {
      dxdy=dx[0][i]*dx[1][j];
      for(k=0;k<3;k++)
      {
        if(neighbors.neighbors[i][j][k])
        {
          weight+=Real(dxdy*dx[2][k]*neighbors.neighbors[i][j][k]->nodeData.centerWeightContribution);
        }
      }
    }
  }
  return Real(1.0/weight);
}

template<int Degree>
inline int Octree<Degree>::NonLinearUpdateWeightContribution(TreeOctNode* node,const Point3D<Real>& position,const Real& weight)
{
  int i,j,k;
  TreeOctNode::Neighbors& neighbors=neighborKey.setNeighbors(node);
  double x,dxdy,dx[DIMENSION][3];
  double twidth;
  Point3D<Real> center;
  Real w;
  node->centerAndWidth(center,w);
  twidth=w;

  for(i=0;i<DIMENSION;i++)
  {
    x=(center.coords[i]-position.coords[i]-twidth)/twidth;
    dx[i][0]=1.125+1.500*x+0.500*x*x;
    x=(center.coords[i]-position.coords[i])/twidth;
    dx[i][1]=0.750        -      x*x;
    dx[i][2]=1.0-dx[i][1]-dx[i][0];
  }

  for(i=0;i<3;i++)
  {
    for(j=0;j<3;j++)
    {
      dxdy=dx[0][i]*dx[1][j]*weight;
      for(k=0;k<3;k++)
      {
        if(neighbors.neighbors[i][j][k]){neighbors.neighbors[i][j][k]->nodeData.centerWeightContribution+=Real(dxdy*dx[2][k]);}
      }
    }
  }
  return 0;
}

template<int Degree>
inline int Octree<Degree>::setTree(vtkSmartPointer<vtkPolyData> data,const int& maxDepth,
                                   const int& kernelDepth,const Real& samplesPerNode,const Real& scaleFactor,Point3D<Real>& center,Real& scale,
                                   const int& resetSamples,const int& useConfidence)
{

  vtkSmartPointer<vtkFloatArray> dataNormals =
    vtkFloatArray::SafeDownCast(data->GetPointData()->GetNormals());
  if(!dataNormals)
  {
    //vtkErrorMacro("The data set does not contain normals!");
    cout << "The data set does not contain normals! This is a fatal error!" << endl;
    return 0;
  }

  Point3D<Real> min,max,position,normal,myCenter;

  Real myWidth;

  TreeOctNode* temp;
  int splatDepth=0;

  TreeNodeData::UseIndex=1;
  neighborKey.set(maxDepth);
  splatDepth=kernelDepth;
  if(splatDepth<0)
  {
    splatDepth=0;
  }

  // Get the center and scale
  double bounds[6];
  data->GetBounds(bounds); //xmin, xmax, ymin, ymax, zmin, zmax

  min.coords[0] = bounds[0];
  max.coords[0] = bounds[1];
  min.coords[1] = bounds[2];
  max.coords[1] = bounds[3];
  min.coords[2] = bounds[4];
  max.coords[2] = bounds[5];

  DumpOutput("Setting bounding box\n");

  for(unsigned int i = 0; i < DIMENSION; i++)
  {
    if(scale<max.coords[i]-min.coords[i])
    {
      scale=Real(max.coords[i]-min.coords[i]);
    }
    center.coords[i]=Real(max.coords[i]+min.coords[i])/2.0;
  }

  scale*=scaleFactor;
  for(unsigned int dim = 0; dim < DIMENSION; dim++)
  {
    center.coords[dim] -= scale/2.0;
  }

  if(splatDepth>0)
  {
    DumpOutput("Setting sample weights\n");

    for(vtkIdType p = 0; p < data->GetNumberOfPoints(); p++)
    {
      double coordinate[3];
      data->GetPoint(p, coordinate);

      double n[3];
      dataNormals->GetTuple(p, n);

      for(unsigned int dim = 0; dim < DIMENSION; dim++)
      {
        position.coords[dim]=(coordinate[dim]-center.coords[dim])/scale;
        normal.coords[dim]=n[dim];
      }

      myCenter.coords[0]=myCenter.coords[1]=myCenter.coords[2]=Real(0.5);
      myWidth=Real(1.0);
      for(unsigned int dim = 0; dim < DIMENSION; dim++)
      {
        if(position.coords[dim]<myCenter.coords[dim]-myWidth/2.0 || position.coords[dim]>myCenter.coords[dim]+myWidth/2.0)
        {
          break;
        }
      }

      temp=&tree;
      int d=0;
      Real weight=Real(1.0);
      if(useConfidence)
      {
        weight=Real(Length(normal));
      }
      while(d<splatDepth)
      {
        NonLinearUpdateWeightContribution(temp,position,weight);
        if(!temp->children)
        {
          temp->initChildren();
        }
        int cIndex=TreeOctNode::CornerIndex(myCenter,position);
        temp=&temp->children[cIndex];
        myWidth/=2;
        if(cIndex&1)
        {
          myCenter.coords[0]+=myWidth/2;
        }
        else
        {
          myCenter.coords[0]-=myWidth/2;
        }
        if(cIndex&2)
        {
          myCenter.coords[1]+=myWidth/2;
        }
        else
        {
          myCenter.coords[1]-=myWidth/2;
        }
        if(cIndex&4)
        {
          myCenter.coords[2]+=myWidth/2;
        }
        else
        {
          myCenter.coords[2]-=myWidth/2;
        }
        d++;
      }
      NonLinearUpdateWeightContribution(temp,position,weight);
    }
  }

  DumpOutput("Adding Points and Normals\n");
  this->normals=new std::vector<Point3D<Real> >();

  for(vtkIdType p = 0; p < data->GetNumberOfPoints(); p++)
  {
    double coordinate[3];
    data->GetPoint(p, coordinate);

    double n[3];
    dataNormals->GetTuple(p, n);

    for(unsigned int dim = 0; dim < DIMENSION; dim++)
    {
      position.coords[dim]=(coordinate[dim]-center.coords[dim])/scale;
      normal.coords[dim]=n[dim];
    }

    myCenter.coords[0]=myCenter.coords[1]=myCenter.coords[2]=Real(0.5);
    myWidth=Real(1.0);

    for(unsigned int dim = 0; dim < DIMENSION; dim++)
    {
      if(position.coords[dim]<myCenter.coords[dim]-myWidth/2.0 || position.coords[dim]>myCenter.coords[dim]+myWidth/2.0)
      {
        break;
      }
    }

    Real l=Real(Length(normal));
    if(l<EPSILON)
    {
      continue;
    }
    if(!useConfidence)
    {
      normal.coords[0]/=l;
      normal.coords[1]/=l;
      normal.coords[2]/=l;
    }
    l=Real(2<<maxDepth);
    normal.coords[0]*=l;
    normal.coords[1]*=l;
    normal.coords[2]*=l;

    if(resetSamples && samplesPerNode>0 && splatDepth)
    {
      NonLinearSplatOrientedPoint(position,normal,splatDepth,samplesPerNode,1,maxDepth);
    }
    else
    {
      Real alpha=1;
      temp=&tree;
      int d=0;
      if(splatDepth)
      {
        while(d<splatDepth)
        {
          int cIndex=TreeOctNode::CornerIndex(myCenter,position);
          temp=&temp->children[cIndex];
          myWidth/=2;
          if(cIndex&1){myCenter.coords[0]+=myWidth/2;}
          else		{myCenter.coords[0]-=myWidth/2;}
          if(cIndex&2){myCenter.coords[1]+=myWidth/2;}
          else		{myCenter.coords[1]-=myWidth/2;}
          if(cIndex&4){myCenter.coords[2]+=myWidth/2;}
          else		{myCenter.coords[2]-=myWidth/2;}
          d++;
        }
        alpha=NonLinearGetSampleWeight(temp,position);
      }
      for(unsigned int dim = 0; dim < DIMENSION; dim++)
      {
        normal.coords[dim] *= alpha;
      }
      while(d<maxDepth)
      {
        if(!temp->children)
        {
          temp->initChildren();
        }
        int cIndex=TreeOctNode::CornerIndex(myCenter,position);
        temp=&temp->children[cIndex];
        myWidth/=2;
        if(cIndex&1)
        {
          myCenter.coords[0]+=myWidth/2;
        }
        else
        {
          myCenter.coords[0]-=myWidth/2;
        }
        if(cIndex&2)
        {
          myCenter.coords[1]+=myWidth/2;
        }
        else
        {
          myCenter.coords[1]-=myWidth/2;
        }
        if(cIndex&4)
        {
          myCenter.coords[2]+=myWidth/2;
        }
        else
        {
          myCenter.coords[2]-=myWidth/2;
        }
        d++;
      }
      NonLinearSplatOrientedPoint(temp,position,normal);
    }
  }

  return 1; //success? used to return cnt
}

template<int Degree>
inline void Octree<Degree>::setFunctionData(const PPolynomial<Degree>& ReconstructionFunction,	const int& maxDepth,const int& normalize,const Real& normalSmooth)
{

  radius=Real(fabs(ReconstructionFunction.polys[0].start));
  width=int(double(radius+0.5-EPSILON)*2);
  if(normalSmooth>0){postNormalSmooth=normalSmooth;}
  fData.set(maxDepth,ReconstructionFunction,normalize,1);
}

template<int Degree>
inline void Octree<Degree>::finalize1(const int& refineNeighbors)
{
  TreeOctNode* temp;

  if(refineNeighbors>=0){
    RefineFunction rf;
    temp=tree.nextNode();
    while(temp){
      if(temp->nodeData.nodeIndex>=0 && Length((*normals)[temp->nodeData.nodeIndex])>EPSILON){
        rf.depth=temp->depth()-refineNeighbors;
        TreeOctNode::ProcessMaxDepthNodeAdjacentNodes(fData.depth,temp,2*width,&tree,1,temp->depth()-refineNeighbors,&rf);
      }
      temp=tree.nextNode(temp);
    }
  }
  else if(refineNeighbors==-1234){
    temp=tree.nextLeaf();
    while(temp){
      if(!temp->children && temp->depth()<fData.depth){temp->initChildren();}
      temp=tree.nextLeaf(temp);
    }
  }
}

template<int Degree>
inline void Octree<Degree>::finalize2(const int& refineNeighbors)
{
  TreeOctNode* temp;

  if(refineNeighbors>=0){
    RefineFunction rf;
    temp=tree.nextNode();
    while(temp){
      if(fabs(temp->nodeData.value)>EPSILON){
        rf.depth=temp->depth()-refineNeighbors;
        TreeOctNode::ProcessMaxDepthNodeAdjacentNodes(fData.depth,temp,2*width,&tree,1,temp->depth()-refineNeighbors,&rf);
      }
      temp=tree.nextNode(temp);
    }
  }
}
template <int Degree>
inline Real Octree<Degree>::GetDivergence(const int idx[DIMENSION],const Point3D<Real>& normal) const
{
  double dot=fData.dotTable[idx[0]]*fData.dotTable[idx[1]]*fData.dotTable[idx[2]];
  return Real(dot*(fData.dDotTable[idx[0]]*normal.coords[0]+fData.dDotTable[idx[1]]*normal.coords[1]+fData.dDotTable[idx[2]]*normal.coords[2]));
}

template<int Degree>
inline Real Octree<Degree>::GetLaplacian(const int idx[DIMENSION]) const
{
  return Real(fData.dotTable[idx[0]]*fData.dotTable[idx[1]]*fData.dotTable[idx[2]]*(fData.d2DotTable[idx[0]]+fData.d2DotTable[idx[1]]+fData.d2DotTable[idx[2]]));
}

template<int Degree>
inline Real Octree<Degree>::GetDotProduct(const int idx[DIMENSION]) const
{
  return Real(fData.dotTable[idx[0]]*fData.dotTable[idx[1]]*fData.dotTable[idx[2]]);
}

template<int Degree>
inline int Octree<Degree>::GetFixedDepthLaplacian(SparseSymmetricMatrix<float>& matrix,const int& depth,const SortedTreeNodes& sNodes)
{
  LaplacianMatrixFunction mf;
  mf.ot=this;
  mf.offset=sNodes.nodeCount[depth];
  matrix.Resize(sNodes.nodeCount[depth+1]-sNodes.nodeCount[depth]);
  mf.rowElements=(MatrixEntry<float>*)malloc(sizeof(MatrixEntry<float>)*matrix.rows);
  for(int i=sNodes.nodeCount[depth];i<sNodes.nodeCount[depth+1];i++){
    mf.elementCount=0;
    mf.d2=int(sNodes.treeNodes[i]->d);
    mf.x2=int(sNodes.treeNodes[i]->off[0]);
    mf.y2=int(sNodes.treeNodes[i]->off[1]);
    mf.z2=int(sNodes.treeNodes[i]->off[2]);
    mf.index[0]=mf.x2;
    mf.index[1]=mf.y2;
    mf.index[2]=mf.z2;
    TreeOctNode::ProcessTerminatingNodeAdjacentNodes(fData.depth,sNodes.treeNodes[i],2*width-1,&tree,1,&mf);
    matrix.SetRowSize(i-sNodes.nodeCount[depth],mf.elementCount);
    memcpy(matrix.m_ppElements[i-sNodes.nodeCount[depth]],mf.rowElements,sizeof(MatrixEntry<float>)*mf.elementCount);
  }
  free(mf.rowElements);
  return 1;
}

template<int Degree>
inline int Octree<Degree>::GetRestrictedFixedDepthLaplacian(SparseSymmetricMatrix<float>& matrix,const int& depth,const int* entries,const int& entryCount,
                                                            const TreeOctNode* rNode,const Real& iRadius,
                                                            const SortedTreeNodes& sNodes){
  (void) depth;

  int i;
  RestrictedLaplacianMatrixFunction mf;
  //NOTE: myRadius was unused (so I commented it)
  //Real myRadius=int(2*iRadius-ROUND_EPS)+ROUND_EPS;
  mf.ot=this;
  mf.radius=iRadius;
  rNode->depthAndOffset(mf.depth,mf.offset);
  matrix.Resize(entryCount);
  mf.rowElements=(MatrixEntry<float>*)malloc(sizeof(MatrixEntry<float>)*matrix.rows);
  for(i=0;i<entryCount;i++){sNodes.treeNodes[entries[i]]->nodeData.nodeIndex=i;}
  for(i=0;i<entryCount;i++){
    mf.elementCount=0;
    mf.index[0]=int(sNodes.treeNodes[entries[i]]->off[0]);
    mf.index[1]=int(sNodes.treeNodes[entries[i]]->off[1]);
    mf.index[2]=int(sNodes.treeNodes[entries[i]]->off[2]);
    TreeOctNode::ProcessTerminatingNodeAdjacentNodes(fData.depth,sNodes.treeNodes[entries[i]],2*width-1,&tree,1,&mf);
    matrix.SetRowSize(i,mf.elementCount);
    memcpy(matrix.m_ppElements[i],mf.rowElements,sizeof(MatrixEntry<float>)*mf.elementCount);
  }
  for(i=0;i<entryCount;i++){sNodes.treeNodes[entries[i]]->nodeData.nodeIndex=entries[i];}
  free(mf.rowElements);
  return 1;
}

template<int Degree>
inline int Octree<Degree>::LaplacianMatrixIteration(const int& subdivideDepth)
{
  int i,iter=0;
  SortedTreeNodes sNodes;
  fData.setDotTables(fData.D2_DOT_FLAG);
  sNodes.set(tree,1);

  SparseMatrix<float>::SetAllocator(MEMORY_ALLOCATOR_BLOCK_SIZE);

  sNodes.treeNodes[0]->nodeData.value=0;
  for(i=1;i<sNodes.maxDepth;i++)
  {
    DumpOutput("Depth: %d/%d\n",i,sNodes.maxDepth-1);

    if(subdivideDepth>0)
    {
      iter+=SolveFixedDepthMatrix(i,subdivideDepth,sNodes);
    }
    else
    {
      iter+=SolveFixedDepthMatrix(i,sNodes);
    }
  }
  SparseMatrix<float>::MatrixAllocator.reset();
  fData.clearDotTables(fData.DOT_FLAG | fData.D_DOT_FLAG | fData.D2_DOT_FLAG);
  return iter;
}

template<int Degree>
inline int Octree<Degree>::SolveFixedDepthMatrix(const int& depth,const SortedTreeNodes& sNodes)
{
  int i,iter=0;
  Vector<double> V,Solution;
  SparseSymmetricMatrix<Real> matrix;
  Real myRadius;
  Real dx,dy,dz;
  int x1,x2,y1,y2,z1,z2;
  Vector<Real> Diagonal;

  V.Resize(sNodes.nodeCount[depth+1]-sNodes.nodeCount[depth]);
  for(i=sNodes.nodeCount[depth];i<sNodes.nodeCount[depth+1];i++)
  {
    V[i-sNodes.nodeCount[depth]]=sNodes.treeNodes[i]->nodeData.value;
  }
  SparseSymmetricMatrix<float>::MatrixAllocator.rollBack();
  GetFixedDepthLaplacian(matrix,depth,sNodes);
  //DumpOutput("\tMatrix entries: %d / %d^2 = %.4f%%\n",matrix.Entries(),matrix.rows,100.0*(matrix.Entries()/double(matrix.rows))/matrix.rows);
//	DumpOutput("\tMemory Usage: %.3f MB\n",float(MemoryUsage()));
  iter+=SparseSymmetricMatrix<Real>::Solve(matrix,V,int(pow(matrix.rows,ITERATION_POWER)),Solution,double(EPSILON),1);
  for(i=sNodes.nodeCount[depth];i<sNodes.nodeCount[depth+1];i++)
  {
    sNodes.treeNodes[i]->nodeData.value=Real(Solution[i-sNodes.nodeCount[depth]]);
  }

  myRadius=Real(radius+ROUND_EPS-0.5);
  myRadius /=(1<<depth);

  if(depth<sNodes.maxDepth-1)
  {
    LaplacianProjectionFunction pf;
    TreeOctNode *node1,*node2;
    pf.ot=this;
    int idx1,idx2,off=sNodes.nodeCount[depth];
    // First pass: idx2 is the solution coefficient propogated
    for(i=0;i<matrix.rows;i++)
    {
      idx1=i;
      node1=sNodes.treeNodes[idx1+off];
      if(!node1->children){continue;}
      x1=int(node1->off[0]);
      y1=int(node1->off[1]);
      z1=int(node1->off[2]);
      for(int j=0;j<matrix.rowSizes[i];j++)
      {
        idx2=matrix.m_ppElements[i][j].N;
        node2=sNodes.treeNodes[idx2+off];
        x2=int(node2->off[0]);
        y2=int(node2->off[1]);
        z2=int(node2->off[2]);
        pf.value=Solution[idx2];
        pf.index[0]=x2;
        pf.index[1]=y2;
        pf.index[2]=z2;
        dx=Real(x2-x1)/(1<<depth);
        dy=Real(y2-y1)/(1<<depth);
        dz=Real(z2-z1)/(1<<depth);
        if(fabs(dx)<myRadius && fabs(dy)<myRadius && fabs(dz)<myRadius)
        {
          node1->processNodeNodes(node2,&pf,0);
        }
        else
        {
          TreeOctNode::ProcessNodeAdjacentNodes(fData.depth,node2,width,node1,width,&pf,0);
        }
      }
    }
    // Second pass: idx1 is the solution coefficient propogated
    for(i=0;i<matrix.rows;i++)
    {
      idx1=i;
      node1=sNodes.treeNodes[idx1+off];
      x1=int(node1->off[0]);
      y1=int(node1->off[1]);
      z1=int(node1->off[2]);
      pf.value=Solution[idx1];
      pf.index[0]=x1;
      pf.index[1]=y1;
      pf.index[2]=z1;
      for(int j=0;j<matrix.rowSizes[i];j++)
      {
        idx2=matrix.m_ppElements[i][j].N;
        node2=sNodes.treeNodes[idx2+off];
        if(idx1!=idx2 && node2->children)
        {
          x2=int(node2->off[0]);
          y2=int(node2->off[1]);
          z2=int(node2->off[2]);
          dx=Real(x1-x2)/(1<<depth);
          dy=Real(y1-y2)/(1<<depth);
          dz=Real(z1-z2)/(1<<depth);
          if(fabs(dx)<myRadius && fabs(dy)<myRadius && fabs(dz)<myRadius)
          {
            node2->processNodeNodes(node1,&pf,0);
          }
          else
          {
            TreeOctNode::ProcessNodeAdjacentNodes(fData.depth,node1,width,node2,width,&pf,0);
          }
        }
      }
    }
  }
  //DumpOutput("\tGot / Solved / Updated in: %6.3f / %6.3f / %6.3f\n",gTime,sTime,uTime);
  return iter;
}

template<int Degree>
inline int Octree<Degree>::SolveFixedDepthMatrix(const int& depth,const int& startingDepth,const SortedTreeNodes& sNodes)
{
  int i,j,d,iter=0;
  SparseSymmetricMatrix<Real> matrix;
  AdjacencySetFunction asf;
  AdjacencyCountFunction acf;
  Vector<Real> Values;
  Vector<double> SubValues,SubSolution;
  Real myRadius,myRadius2;
  Real dx,dy,dz;
  Vector<Real> Diagonal;

  if(startingDepth>=depth){return SolveFixedDepthMatrix(depth,sNodes);}

  Values.Resize(sNodes.nodeCount[depth+1]-sNodes.nodeCount[depth]);

  for(i=sNodes.nodeCount[depth];i<sNodes.nodeCount[depth+1];i++){
    Values[i-sNodes.nodeCount[depth]]=sNodes.treeNodes[i]->nodeData.value;
    sNodes.treeNodes[i]->nodeData.value=0;
  }

  myRadius=2*radius-Real(0.5);
  myRadius=int(myRadius-ROUND_EPS)+ROUND_EPS;
  myRadius2=Real(radius+ROUND_EPS-0.5);
  d=depth-startingDepth;
  for(i=sNodes.nodeCount[d];i<sNodes.nodeCount[d+1];i++){
    TreeOctNode* temp;
    // Get all of the entries associated to the subspace
    acf.adjacencyCount=0;
    temp=sNodes.treeNodes[i]->nextNode();
    while(temp){
      if(temp->depth()==depth){
        acf.Function(temp,temp);
        temp=sNodes.treeNodes[i]->nextBranch(temp);
      }
      else{temp=sNodes.treeNodes[i]->nextNode(temp);}
    }
    for(j=sNodes.nodeCount[d];j<sNodes.nodeCount[d+1];j++){
      if(i==j){continue;}
      TreeOctNode::ProcessFixedDepthNodeAdjacentNodes(fData.depth,sNodes.treeNodes[i],1,sNodes.treeNodes[j],2*width-1,depth,&acf);
    }
    if(!acf.adjacencyCount){continue;}
    asf.adjacencies=new int[acf.adjacencyCount];
    asf.adjacencyCount=0;
    temp=sNodes.treeNodes[i]->nextNode();
    while(temp){
      if(temp->depth()==depth){
        asf.Function(temp,temp);
        temp=sNodes.treeNodes[i]->nextBranch(temp);
      }
      else{temp=sNodes.treeNodes[i]->nextNode(temp);}
    }
    for(j=sNodes.nodeCount[d];j<sNodes.nodeCount[d+1];j++){
      if(i==j){continue;}
      TreeOctNode::ProcessFixedDepthNodeAdjacentNodes(fData.depth,sNodes.treeNodes[i],1,sNodes.treeNodes[j],2*width-1,depth,&asf);
    }

    DumpOutput("\tNodes[%d/%d]: %d\n",i-sNodes.nodeCount[d]+1,sNodes.nodeCount[d+1]-sNodes.nodeCount[d],asf.adjacencyCount);
    // Get the associated vector
    SubValues.Resize(asf.adjacencyCount);
    for(j=0;j<asf.adjacencyCount;j++){SubValues[j]=Values[asf.adjacencies[j]-sNodes.nodeCount[depth]];}
    SubSolution.Resize(asf.adjacencyCount);
    for(j=0;j<asf.adjacencyCount;j++){SubSolution[j]=sNodes.treeNodes[asf.adjacencies[j]]->nodeData.value;}
    // Get the associated matrix
    SparseSymmetricMatrix<float>::MatrixAllocator.rollBack();
    GetRestrictedFixedDepthLaplacian(matrix,depth,asf.adjacencies,asf.adjacencyCount,sNodes.treeNodes[i],myRadius,sNodes);
    //DumpOutput("\t\tMatrix entries: %d / %d^2 = %.4f%%\n",matrix.Entries(),matrix.rows,100.0*(matrix.Entries()/double(matrix.rows))/matrix.rows);
    //DumpOutput("\t\tMemory Usage: %.3f MB\n",float(MemoryUsage()));

    // Solve the matrix
    iter+=SparseSymmetricMatrix<Real>::Solve(matrix,SubValues,int(pow(matrix.rows,ITERATION_POWER)),SubSolution,double(EPSILON),0);

    LaplacianProjectionFunction lpf;
    lpf.ot=this;

    // Update the solution for all nodes in the sub-tree
    for(j=0;j<asf.adjacencyCount;j++)
    {
      temp=sNodes.treeNodes[asf.adjacencies[j]];
      while(temp->depth()>sNodes.treeNodes[i]->depth())
      {
        temp=temp->parent;
      }
      if(temp->nodeData.nodeIndex>=sNodes.treeNodes[i]->nodeData.nodeIndex)
      {
        sNodes.treeNodes[asf.adjacencies[j]]->nodeData.value=Real(SubSolution[j]);
      }
    }
    //NOTE: t was unused (so I commented it)
    //double t=Time();
    // Update the values in the next depth
    int x1,x2,y1,y2,z1,z2;
    if(depth<sNodes.maxDepth-1)
    {
      int idx1,idx2;
      TreeOctNode *node1,*node2;
      // First pass: idx2 is the solution coefficient propogated
      for(j=0;j<matrix.rows;j++)
      {
        idx1=asf.adjacencies[j];
        node1=sNodes.treeNodes[idx1];
        if(!node1->children)
        {
          continue;
        }
        x1=int(node1->off[0]);
        y1=int(node1->off[1]);
        z1=int(node1->off[2]);

        for(int k=0;k<matrix.rowSizes[j];k++)
        {
          idx2=asf.adjacencies[matrix.m_ppElements[j][k].N];
          node2=sNodes.treeNodes[idx2];
          temp=node2;
          while(temp->depth()>d)
          {
            temp=temp->parent;
          }
          if(temp!=sNodes.treeNodes[i])
          {
            continue;
          }
          lpf.value=Real(SubSolution[matrix.m_ppElements[j][k].N]);
          x2=int(node2->off[0]);
          y2=int(node2->off[1]);
          z2=int(node2->off[2]);
          lpf.index[0]=x2;
          lpf.index[1]=y2;
          lpf.index[2]=z2;
          dx=Real(x2-x1)/(1<<depth);
          dy=Real(y2-y1)/(1<<depth);
          dz=Real(z2-z1)/(1<<depth);
          if(fabs(dx)<myRadius2 && fabs(dy)<myRadius2 && fabs(dz)<myRadius2)
          {
            node1->processNodeNodes(node2,&lpf,0);
          }
          else
          {
            TreeOctNode::ProcessNodeAdjacentNodes(fData.depth,node2,width,node1,width,&lpf,0);
          }
        }
      }
      // Second pass: idx1 is the solution coefficient propogated
      for(j=0;j<matrix.rows;j++)
      {
        idx1=asf.adjacencies[j];
        node1=sNodes.treeNodes[idx1];
        temp=node1;
        while(temp->depth()>d)
        {
          temp=temp->parent;
        }
        if(temp!=sNodes.treeNodes[i])
        {
          continue;
        }
        x1=int(node1->off[0]);
        y1=int(node1->off[1]);
        z1=int(node1->off[2]);

        lpf.value=Real(SubSolution[j]);
        lpf.index[0]=x1;
        lpf.index[1]=y1;
        lpf.index[2]=z1;
        for(int k=0;k<matrix.rowSizes[j];k++)
        {
          idx2=asf.adjacencies[matrix.m_ppElements[j][k].N];
          node2=sNodes.treeNodes[idx2];
          if(!node2->children)
          {
            continue;
          }

          if(idx1!=idx2)
          {
            x2=int(node2->off[0]);
            y2=int(node2->off[1]);
            z2=int(node2->off[2]);
            dx=Real(x1-x2)/(1<<depth);
            dy=Real(y1-y2)/(1<<depth);
            dz=Real(z1-z2)/(1<<depth);
            if(fabs(dx)<myRadius2 && fabs(dy)<myRadius2 && fabs(dz)<myRadius2)
            {
              node2->processNodeNodes(node1,&lpf,0);
            }
            else
            {
              TreeOctNode::ProcessNodeAdjacentNodes(fData.depth,node1,width,node2,width,&lpf,0);
            }
          }
        }
      }
    }

    //DumpOutput("\t\tGot / Solved / Updated in: %6.3f / %6.3f / %6.3f\n",gTime,sTime,uTime);
    delete[] asf.adjacencies;
  }
  return iter;
}

template<int Degree>
inline int Octree<Degree>::HasNormals(TreeOctNode* node,const Real& epsilon)
{
  int hasNormals=0;
  if(node->nodeData.nodeIndex>=0 && Length((*normals)[node->nodeData.nodeIndex])>epsilon)
  {
    hasNormals=1;
  }
  if(node->children)
  {
    for(int i=0;i<Cube::CORNERS && !hasNormals;i++)
    {
      hasNormals|=HasNormals(&node->children[i],epsilon);
    }
  }

  return hasNormals;
}

template<int Degree>
inline void Octree<Degree>::ClipTree(void)
{
  TreeOctNode* temp;
  temp=tree.nextNode();
  while(temp)
  {
    if(temp->children)
    {
      int hasNormals=0;
      for(int i=0;i<Cube::CORNERS && !hasNormals;i++)
      {
        hasNormals=HasNormals(&temp->children[i],EPSILON);
      }
      if(!hasNormals)
      {
        temp->children=NULL;
      }
    }
    temp=tree.nextNode(temp);
  }
}

template<int Degree>
inline void Octree<Degree>::SetLaplacianWeights(void)
{
  TreeOctNode* temp;

  fData.setDotTables(fData.DOT_FLAG | fData.D_DOT_FLAG);
  DivergenceFunction df;
  df.ot=this;
  temp=tree.nextNode();
  while(temp){
    if(temp->nodeData.nodeIndex<0 || Length((*normals)[temp->nodeData.nodeIndex])<=EPSILON){
      temp=tree.nextNode(temp);
      continue;
    }
    //NOTE: d was unused (so I commented it)
    // int d=temp->depth();
    df.normal=(*normals)[temp->nodeData.nodeIndex];
    df.index[0]=int(temp->off[0]);
    df.index[1]=int(temp->off[1]);
    df.index[2]=int(temp->off[2]);
    TreeOctNode::ProcessNodeAdjacentNodes(fData.depth,temp,width,&tree,width,&df);
    temp=tree.nextNode(temp);
  }
  fData.clearDotTables(fData.D_DOT_FLAG);
  temp=tree.nextNode();
  while(temp){
    if(temp->nodeData.nodeIndex<0){temp->nodeData.centerWeightContribution=0;}
    else{temp->nodeData.centerWeightContribution=Real(Length((*normals)[temp->nodeData.nodeIndex]));}
    temp=tree.nextNode(temp);
  }
//	MemoryUsage();

  delete normals;
  normals=NULL;
}

template<int Degree>
inline void Octree<Degree>::DivergenceFunction::Function(TreeOctNode* node1,const TreeOctNode* node2)
{
  (void) node2;

  Point3D<Real> n=normal;
  if(FunctionData<Degree,Real>::SymmetricIndex(index[0],int(node1->off[0]),scratch[0])){n.coords[0]=-n.coords[0];}
  if(FunctionData<Degree,Real>::SymmetricIndex(index[1],int(node1->off[1]),scratch[1])){n.coords[1]=-n.coords[1];}
  if(FunctionData<Degree,Real>::SymmetricIndex(index[2],int(node1->off[2]),scratch[2])){n.coords[2]=-n.coords[2];}
  double dot=ot->fData.dotTable[scratch[0]]*ot->fData.dotTable[scratch[1]]*ot->fData.dotTable[scratch[2]];
  node1->nodeData.value+=Real(dot*(ot->fData.dDotTable[scratch[0]]*n.coords[0]+ot->fData.dDotTable[scratch[1]]*n.coords[1]+ot->fData.dDotTable[scratch[2]]*n.coords[2]));
}

template<int Degree>
inline void Octree<Degree>::LaplacianProjectionFunction::Function(TreeOctNode* node1,const TreeOctNode* node2)
{
  (void) node2;

  scratch[0]=FunctionData<Degree,Real>::SymmetricIndex(index[0],int(node1->off[0]));
  scratch[1]=FunctionData<Degree,Real>::SymmetricIndex(index[1],int(node1->off[1]));
  scratch[2]=FunctionData<Degree,Real>::SymmetricIndex(index[2],int(node1->off[2]));
  node1->nodeData.value-=Real(ot->GetLaplacian(scratch)*value);
}

template<int Degree>
inline void Octree<Degree>::AdjacencyCountFunction::Function(const TreeOctNode* node1,const TreeOctNode* node2)
{
  (void) node1;
  (void) node2;
  adjacencyCount++;
}

template<int Degree>
inline void Octree<Degree>::AdjacencySetFunction::Function(const TreeOctNode* node1,const TreeOctNode* node2)
{
  (void) node2;
  adjacencies[adjacencyCount++]=node1->nodeData.nodeIndex;
}

template<int Degree>
inline void Octree<Degree>::RefineFunction::Function(TreeOctNode* node1,const TreeOctNode* node2)
{
  (void) node2;
  if(!node1->children && node1->depth()<depth){node1->initChildren();}
}

template<int Degree>
inline void Octree<Degree>::FaceEdgesFunction::Function(const TreeOctNode* node1,const TreeOctNode* node2)
{
  (void) node2;

  if(!node1->children && MarchingCubes::HasRoots(node1->nodeData.mcIndex)){
    RootInfo ri1,ri2;
    std::unordered_map<long long,std::pair<RootInfo,int> >::iterator iter;
    int isoTri[DIMENSION*MarchingCubes::MAX_TRIANGLES];
    int count=MarchingCubes::AddTriangleIndices(node1->nodeData.mcIndex,isoTri);

    for(int j=0;j<count;j++){
      for(int k=0;k<3;k++){
        if(fIndex==Cube::FaceAdjacentToEdges(isoTri[j*3+k],isoTri[j*3+((k+1)%3)])){
          if(GetRootIndex(node1,isoTri[j*3+k],maxDepth,ri1) && GetRootIndex(node1,isoTri[j*3+((k+1)%3)],maxDepth,ri2)){
            edges->push_back(std::pair<long long,long long>(ri2.key,ri1.key));
            iter=vertexCount->find(ri1.key);
            if(iter==vertexCount->end()){
              (*vertexCount)[ri1.key].first=ri1;
              (*vertexCount)[ri1.key].second=0;
            }
            iter=vertexCount->find(ri2.key);
            if(iter==vertexCount->end()){
              (*vertexCount)[ri2.key].first=ri2;
              (*vertexCount)[ri2.key].second=0;
            }
            (*vertexCount)[ri1.key].second--;
            (*vertexCount)[ri2.key].second++;
          }
          else{fprintf(stderr,"Bad Edge 1: %lld %lld\n",ri1.key,ri2.key);}
        }
      }
    }
  }
}

template<int Degree>
inline void Octree<Degree>::PointIndexValueFunction::Function(const TreeOctNode* node)
{
  int idx[DIMENSION];
  idx[0]=index[0]+int(node->off[0]);
  idx[1]=index[1]+int(node->off[1]);
  idx[2]=index[2]+int(node->off[2]);
  value+=node->nodeData.value*   Real( valueTables[idx[0]]* valueTables[idx[1]]* valueTables[idx[2]]);
}

template<int Degree>
inline void Octree<Degree>::PointIndexValueAndNormalFunction::Function(const TreeOctNode* node)
{
  int idx[DIMENSION];
  idx[0]=index[0]+int(node->off[0]);
  idx[1]=index[1]+int(node->off[1]);
  idx[2]=index[2]+int(node->off[2]);
  value+=				node->nodeData.value*   Real( valueTables[idx[0]]* valueTables[idx[1]]* valueTables[idx[2]]);
  normal.coords[0]+=	node->nodeData.value*   Real(dValueTables[idx[0]]* valueTables[idx[1]]* valueTables[idx[2]]);
  normal.coords[1]+=	node->nodeData.value*   Real( valueTables[idx[0]]*dValueTables[idx[1]]* valueTables[idx[2]]);
  normal.coords[2]+=	node->nodeData.value*   Real( valueTables[idx[0]]* valueTables[idx[1]]*dValueTables[idx[2]]);
}

template<int Degree>
inline int Octree<Degree>::LaplacianMatrixFunction::Function(const TreeOctNode* node1,const TreeOctNode* node2)
{
  Real temp;
  int d1=int(node1->d);
  int x1,y1,z1;
  x1=int(node1->off[0]);
  y1=int(node1->off[1]);
  z1=int(node1->off[2]);
  int dDepth=d2-d1;
  int d;
  d=(x2>>dDepth)-x1;
  if(d<0){return 0;}
  if(!dDepth){
    if(!d){
      d=y2-y1;
      if(d<0){return 0;}
      else if(!d){
        d=z2-z1;
        if(d<0){return 0;}
      }
    }
    scratch[0]=FunctionData<Degree,Real>::SymmetricIndex(index[0],x1);
    scratch[1]=FunctionData<Degree,Real>::SymmetricIndex(index[1],y1);
    scratch[2]=FunctionData<Degree,Real>::SymmetricIndex(index[2],z1);
    temp=ot->GetLaplacian(scratch);
    if(node1==node2){temp/=2;}
    if(fabs(temp)>EPSILON){
      rowElements[elementCount].Value=temp;
      rowElements[elementCount].N=node1->nodeData.nodeIndex-offset;
      elementCount++;
    }
    return 0;
  }
  return 1;
}

template<int Degree>
inline int Octree<Degree>::RestrictedLaplacianMatrixFunction::Function(const TreeOctNode* node1,const TreeOctNode* node2)
{
  int d1,d2,off1[3],off2[3];
  node1->depthAndOffset(d1,off1);
  node2->depthAndOffset(d2,off2);
  int dDepth=d2-d1;
  int d;
  d=(off2[0]>>dDepth)-off1[0];
  if(d<0){return 0;}

  if(!dDepth){
    if(!d){
      d=off2[1]-off1[1];
      if(d<0){return 0;}
      else if(!d){
        d=off2[2]-off1[2];
        if(d<0){return 0;}
      }
    }
    // Since we are getting the restricted matrix, we don't want to propogate out to terms that don't contribute...
    if(!TreeOctNode::Overlap2(depth,offset,0.5,d1,off1,radius)){return 0;}
    scratch[0]=FunctionData<Degree,Real>::SymmetricIndex(index[0],BinaryNode<Real>::Index(d1,off1[0]));
    scratch[1]=FunctionData<Degree,Real>::SymmetricIndex(index[1],BinaryNode<Real>::Index(d1,off1[1]));
    scratch[2]=FunctionData<Degree,Real>::SymmetricIndex(index[2],BinaryNode<Real>::Index(d1,off1[2]));
    Real temp=ot->GetLaplacian(scratch);
    if(node1==node2){temp/=2;}
    if(fabs(temp)>EPSILON){
      rowElements[elementCount].Value=temp;
      rowElements[elementCount].N=node1->nodeData.nodeIndex;
      elementCount++;
    }
    return 0;
  }
  return 1;
}

template<int Degree>
inline void Octree<Degree>::GetMCIsoTriangles(const Real& isoValue,CoredMeshData* mesh,const int& fullDepthIso,const int& nonLinearFit, bool addBarycenter)
{
  TreeOctNode* temp;

  std::unordered_map<long long,int> roots;
  std::unordered_map<long long,std::pair<Real,Point3D<Real> > > *normalHash=new std::unordered_map<long long,std::pair<Real,Point3D<Real> > >();

  SetIsoSurfaceCorners(isoValue,0,fullDepthIso);
  // At the point all of the corner values have been set and all nodes are valid. Now it's just a matter
  // of running marching cubes.

  fData.setValueTables(fData.VALUE_FLAG | fData.D_VALUE_FLAG,0,postNormalSmooth);
  temp=tree.nextLeaf();
  while(temp)
  {
    SetMCRootPositions(temp,0,isoValue,roots,NULL,*normalHash,NULL,NULL,mesh,nonLinearFit);
    temp=tree.nextLeaf(temp);
  }
//	MemoryUsage();

  //DumpOutput("Normal Size: %.2f MB\n",double(sizeof(Point3D<Real>)*normalHash->size())/1000000);
  //DumpOutput("Set %d root positions in: %f\n",mesh->inCorePoints.size(),Time()-t);
  //DumpOutput("Memory Usage: %.3f MB\n",float(MemoryUsage()));

  fData.clearValueTables();
  delete normalHash;

  //DumpOutput("Post deletion size: %.3f MB\n",float(MemoryUsage()));

  // Now get the iso-surfaces, running from finest nodes to coarsest in order to allow for edge propogation from
  // finer faces to coarser ones.
  temp=tree.nextLeaf();
  while(temp)
  {
    GetMCIsoTriangles(temp,mesh,roots,NULL,NULL,0,0,addBarycenter);
    temp=tree.nextLeaf(temp);
  }
  //DumpOutput("Added triangles in: %f\n",Time()-t);
  //DumpOutput("Memory Usage: %.3f MB\n",float(MemoryUsage()));
}

template<int Degree>
inline void Octree<Degree>::GetMCIsoTriangles(const Real& isoValue,const int& subdivideDepth,CoredMeshData* mesh,const int& fullDepthIso,const int& nonLinearFit, bool addBarycenter)
{
  TreeOctNode* temp;
  std::unordered_map<long long,int> boundaryRoots,*interiorRoots;
  std::unordered_map<long long,std::pair<Real,Point3D<Real> > > *boundaryNormalHash,*interiorNormalHash;
  std::vector<Point3D<float> >* interiorPoints;

  int sDepth;
  if(subdivideDepth<=0)
  {
    sDepth=0;
  }
  else
  {
    sDepth=fData.depth-subdivideDepth;
  }
  if(sDepth<0)
  {
    sDepth=0;
  }

  SetIsoSurfaceCorners(isoValue,sDepth,fullDepthIso);
  // At this point all of the corner values have been set and all nodes are valid. Now it's just a matter
  // of running marching cubes.

  boundaryNormalHash=new std::unordered_map<long long,std::pair<Real,Point3D<Real> > >();
  int offSet=0;
  SortedTreeNodes sNodes;
  sNodes.set(tree,0);
  fData.setValueTables(fData.VALUE_FLAG | fData.D_VALUE_FLAG,0,postNormalSmooth);

  // Set the root positions for all leaf nodes below the subdivide threshold
  SetBoundaryMCRootPositions(sDepth,isoValue,boundaryRoots,*boundaryNormalHash,mesh,nonLinearFit);

  for(int i=sNodes.nodeCount[sDepth];i<sNodes.nodeCount[sDepth+1];i++){
    interiorRoots=new std::unordered_map<long long,int>();
    interiorNormalHash=new std::unordered_map<long long,std::pair<Real,Point3D<Real> > >();
    interiorPoints=new std::vector<Point3D<float> >();

    temp=sNodes.treeNodes[i]->nextLeaf();
    while(temp){
      if(MarchingCubes::HasRoots(temp->nodeData.mcIndex)){
        SetMCRootPositions(temp,sDepth,isoValue,boundaryRoots,interiorRoots,*boundaryNormalHash,interiorNormalHash,interiorPoints,mesh,nonLinearFit);
      }
      temp=sNodes.treeNodes[i]->nextLeaf(temp);
    }
    delete interiorNormalHash;

    temp=sNodes.treeNodes[i]->nextLeaf();
    while(temp){
      GetMCIsoTriangles(temp,mesh,boundaryRoots,interiorRoots,interiorPoints,offSet,sDepth,addBarycenter);
      temp=sNodes.treeNodes[i]->nextLeaf(temp);
    }
    delete interiorRoots;
    delete interiorPoints;
    offSet=mesh->outOfCorePointCount();
  }
  delete boundaryNormalHash;

  temp=tree.nextLeaf();
  while(temp){
    if(temp->depth()<sDepth){GetMCIsoTriangles(temp,mesh,boundaryRoots,NULL,NULL,0,0,addBarycenter);}
    temp=tree.nextLeaf(temp);
  }
}

template<int Degree>
inline Real Octree<Degree>::getCenterValue(const TreeOctNode* node)
{
  int idx[3];
  Real value=0;

  neighborKey2.getNeighbors(node);
  VertexData::CenterIndex(node,fData.depth,idx);
  idx[0]*=fData.res;
  idx[1]*=fData.res;
  idx[2]*=fData.res;
  for(int i=0;i<=node->depth();i++){
    for(int j=0;j<3;j++){
      for(int k=0;k<3;k++){
        for(int l=0;l<3;l++){
          const TreeOctNode* n=neighborKey2.neighbors[i].neighbors[j][k][l];
          if(n){
            Real temp=n->nodeData.value;
            value+=temp*Real(
              fData.valueTables[idx[0]+int(n->off[0])]*
              fData.valueTables[idx[1]+int(n->off[1])]*
              fData.valueTables[idx[2]+int(n->off[2])]);
          }
        }
      }
    }
  }
  if(node->children){
    for(int i=0;i<Cube::CORNERS;i++){
      int ii=Cube::AntipodalCornerIndex(i);
      const TreeOctNode* n=&node->children[i];
      while(1){
        value+=n->nodeData.value*Real(
          fData.valueTables[idx[0]+int(n->off[0])]*
          fData.valueTables[idx[1]+int(n->off[1])]*
          fData.valueTables[idx[2]+int(n->off[2])]);
        if(n->children){n=&n->children[ii];}
        else{break;}
      }
    }
  }
  return value;
}

template<int Degree>
inline Real Octree<Degree>::getCornerValue(const TreeOctNode* node,const int& corner)
{
  int idx[3];
  Real value=0;

  neighborKey2.getNeighbors(node);
  VertexData::CornerIndex(node,corner,fData.depth,idx);
  idx[0]*=fData.res;
  idx[1]*=fData.res;
  idx[2]*=fData.res;
  for(int i=0;i<=node->depth();i++){
    for(int j=0;j<3;j++){
      for(int k=0;k<3;k++){
        for(int l=0;l<3;l++){
          const TreeOctNode* n=neighborKey2.neighbors[i].neighbors[j][k][l];
          if(n){
            Real temp=n->nodeData.value;
            value+=temp*Real(
              fData.valueTables[idx[0]+int(n->off[0])]*
              fData.valueTables[idx[1]+int(n->off[1])]*
              fData.valueTables[idx[2]+int(n->off[2])]);
          }
        }
      }
    }
  }
  int x,y,z,d=node->depth();
  Cube::FactorCornerIndex(corner,x,y,z);
  for(int i=0;i<2;i++){
    for(int j=0;j<2;j++){
      for(int k=0;k<2;k++){
        const TreeOctNode* n=neighborKey2.neighbors[d].neighbors[x+i][y+j][z+k];
        if(n){
          int ii=Cube::AntipodalCornerIndex(Cube::CornerIndex(i,j,k));
          while(n->children){
            n=&n->children[ii];
            value+=n->nodeData.value*Real(
              fData.valueTables[idx[0]+int(n->off[0])]*
              fData.valueTables[idx[1]+int(n->off[1])]*
              fData.valueTables[idx[2]+int(n->off[2])]);
          }
        }
      }
    }
  }
  return value;
}

template<int Degree>
inline void Octree<Degree>::getCornerValueAndNormal(const TreeOctNode* node,const int& corner,Real& value,Point3D<Real>& normal)
{
  int idx[3],index[3];
  value=normal.coords[0]=normal.coords[1]=normal.coords[2]=0;

  neighborKey2.getNeighbors(node);
  VertexData::CornerIndex(node,corner,fData.depth,idx);
  idx[0]*=fData.res;
  idx[1]*=fData.res;
  idx[2]*=fData.res;
  for(int i=0;i<=node->depth();i++){
    for(int j=0;j<3;j++){
      for(int k=0;k<3;k++){
        for(int l=0;l<3;l++){
          const TreeOctNode* n=neighborKey2.neighbors[i].neighbors[j][k][l];
          if(n){
            Real temp=n->nodeData.value;
            index[0]=idx[0]+int(n->off[0]);
            index[1]=idx[1]+int(n->off[1]);
            index[2]=idx[2]+int(n->off[2]);
            value+=temp*Real(fData.valueTables[index[0]]*fData.valueTables[index[1]]*fData.valueTables[index[2]]);
            normal.coords[0]+=temp*Real(fData.dValueTables[index[0]]* fData.valueTables[index[1]]* fData.valueTables[index[2]]);
            normal.coords[1]+=temp*Real( fData.valueTables[index[0]]*fData.dValueTables[index[1]]* fData.valueTables[index[2]]);
            normal.coords[2]+=temp*Real( fData.valueTables[index[0]]* fData.valueTables[index[1]]*fData.dValueTables[index[2]]);
          }
        }
      }
    }
  }
  int x,y,z,d=node->depth();
  Cube::FactorCornerIndex(corner,x,y,z);
  for(int i=0;i<2;i++){
    for(int j=0;j<2;j++){
      for(int k=0;k<2;k++){
        const TreeOctNode* n=neighborKey2.neighbors[d].neighbors[x+i][y+j][z+k];
        if(n){
          int ii=Cube::AntipodalCornerIndex(Cube::CornerIndex(i,j,k));
          while(n->children){
            n=&n->children[ii];
            Real temp=n->nodeData.value;
            index[0]=idx[0]+int(n->off[0]);
            index[1]=idx[1]+int(n->off[1]);
            index[2]=idx[2]+int(n->off[2]);
            value+=temp*Real(fData.valueTables[index[0]]*fData.valueTables[index[1]]*fData.valueTables[index[2]]);
            normal.coords[0]+=temp*Real(fData.dValueTables[index[0]]* fData.valueTables[index[1]]* fData.valueTables[index[2]]);
            normal.coords[1]+=temp*Real( fData.valueTables[index[0]]*fData.dValueTables[index[1]]* fData.valueTables[index[2]]);
            normal.coords[2]+=temp*Real( fData.valueTables[index[0]]* fData.valueTables[index[1]]*fData.dValueTables[index[2]]);
          }
        }
      }
    }
  }
}

template<int Degree>
inline Real Octree<Degree>::GetIsoValue(void)
{
  if(this->width<=3){
    TreeOctNode* temp;
    Real isoValue,weightSum,w;

    neighborKey2.set(fData.depth);
    fData.setValueTables(fData.VALUE_FLAG,0);

    isoValue=weightSum=0;
    temp=tree.nextNode();
    while(temp){
      w=temp->nodeData.centerWeightContribution;
      if(w>EPSILON){
        isoValue+=getCenterValue(temp)*w;
        weightSum+=w;
      }
      temp=tree.nextNode(temp);
    }
    return isoValue/weightSum;
  }
  else{
    const TreeOctNode* temp;
    Real isoValue,weightSum,w;
    PointIndexValueFunction cf;

    fData.setValueTables(fData.VALUE_FLAG,0);
    cf.valueTables=fData.valueTables;
    cf.res2=fData.res2;
    isoValue=weightSum=0;
    temp=tree.nextNode();
    while(temp){
      w=temp->nodeData.centerWeightContribution;
      if(w>EPSILON){
        cf.value=0;
        int idx[3];
        VertexData::CenterIndex(temp,fData.depth,idx);
        cf.index[0]=idx[0]*fData.res;
        cf.index[1]=idx[1]*fData.res;
        cf.index[2]=idx[2]*fData.res;
        TreeOctNode::ProcessPointAdjacentNodes(fData.depth,idx,&tree,width,&cf);
        isoValue+=cf.value*w;
        weightSum+=w;
      }
      temp=tree.nextNode(temp);
    }
    return isoValue/weightSum;
  }
}

template<int Degree>
inline void Octree<Degree>::SetIsoSurfaceCorners(const Real& isoValue,const int& subdivideDepth,const int& fullDepthIso)
{
  (void) fullDepthIso;

  int i,j;
  std::unordered_map<long long,Real> values;
  Real cornerValues[Cube::CORNERS];
  PointIndexValueFunction cf;
  TreeOctNode* temp;
  //NOTE: leafCount was unused (so I commented it)
  //int leafCount=tree.leaves();
  long long key;
  SortedTreeNodes *sNodes=new SortedTreeNodes();
  sNodes->set(tree,0);
  temp=tree.nextNode();
  while(temp){
    temp->nodeData.mcIndex=0;
    temp=tree.nextNode(temp);
  }
  TreeNodeData::UseIndex=0;
  // Start by setting the corner values of all the nodes
  cf.valueTables=fData.valueTables;
  cf.res2=fData.res2;
  for(i=0;i<sNodes->nodeCount[subdivideDepth];i++){
    temp=sNodes->treeNodes[i];
    if(!temp->children){
      for(j=0;j<Cube::CORNERS;j++){
        if(this->width<=3){cornerValues[j]=getCornerValue(temp,j);}
        else{
          cf.value=0;
          int idx[3];
          VertexData::CornerIndex(temp,j,fData.depth,idx);
          cf.index[0]=idx[0]*fData.res;
          cf.index[1]=idx[1]*fData.res;
          cf.index[2]=idx[2]*fData.res;
          TreeOctNode::ProcessPointAdjacentNodes(fData.depth,idx,&tree,width,&cf);
          cornerValues[j]=cf.value;
        }
      }
      temp->nodeData.mcIndex=MarchingCubes::GetIndex(cornerValues,isoValue);

      if(temp->parent){
        TreeOctNode* parent=temp->parent;
        int c=int(temp-temp->parent->children);
        int mcid=temp->nodeData.mcIndex&(1<<MarchingCubes::cornerMap[c]);

        if(mcid){
          parent->nodeData.mcIndex|=mcid;
          while(1){
            if(parent->parent && (parent-parent->parent->children)==c){
              parent->parent->nodeData.mcIndex|=mcid;
              parent=parent->parent;
            }
            else{break;}
          }
        }
      }
    }
  }

//	MemoryUsage();

  for(i=sNodes->nodeCount[subdivideDepth];i<sNodes->nodeCount[subdivideDepth+1];i++)
  {
    temp=sNodes->treeNodes[i]->nextLeaf();
    while(temp)
    {
      for(j=0;j<Cube::CORNERS;j++)
      {
        int idx[3];
        key=VertexData::CornerIndex(temp,j,fData.depth,idx);
        cf.index[0]=idx[0]*fData.res;
        cf.index[1]=idx[1]*fData.res;
        cf.index[2]=idx[2]*fData.res;
        if(values.find(key)!=values.end())
        {
          cornerValues[j]=values[key];
        }
        else
        {
          if(this->width<=3)
          {
            values[key]=cornerValues[j]=getCornerValue(temp,j);
          }
          else
          {
            cf.value=0;
            TreeOctNode::ProcessPointAdjacentNodes(fData.depth,idx,&tree,width,&cf);
            values[key]=cf.value;
            cornerValues[j]=cf.value;
          }
        }
      }
      temp->nodeData.mcIndex=MarchingCubes::GetIndex(cornerValues,isoValue);

      if(temp->parent){
        TreeOctNode* parent=temp->parent;
        int c=int(temp-temp->parent->children);
        int mcid=temp->nodeData.mcIndex&(1<<MarchingCubes::cornerMap[c]);

        if(mcid){
          parent->nodeData.mcIndex|=mcid;
          while(1){
            if(parent->parent && (parent-parent->parent->children)==c){
              parent->parent->nodeData.mcIndex|=mcid;
              parent=parent->parent;
            }
            else{break;}
          }
        }
      }

      temp=sNodes->treeNodes[i]->nextLeaf(temp);
    }
    values.clear();
  }
  delete sNodes;
  //DumpOutput("Set corner values in: %f\n",Time()-t);
  //DumpOutput("Memory Usage: %.3f MB\n",float(MemoryUsage()));

  if(subdivideDepth)
  {
    PreValidate(isoValue,fData.depth,subdivideDepth);
  }
}

template<int Degree>
inline void Octree<Degree>::Subdivide(TreeOctNode* node,const Real& isoValue,const int& maxDepth)
{
  int i,j,c[4];
  Real value;
  int cornerIndex2[Cube::CORNERS];
  PointIndexValueFunction cf;
  cf.valueTables=fData.valueTables;
  cf.res2=fData.res2;
  node->initChildren();
  // Since we are allocating blocks, it is possible that some of the memory was pre-allocated with
  // the wrong initialization

  // Now set the corner values for the new children
  // Copy old corner values
  for(i=0;i<Cube::CORNERS;i++){cornerIndex2[i]=node->nodeData.mcIndex&(1<<MarchingCubes::cornerMap[i]);}
  // 8 of 27 corners set

  // Set center corner
  cf.value=0;
  int idx[3];
  VertexData::CenterIndex(node,maxDepth,idx);
  cf.index[0]=idx[0]*fData.res;
  cf.index[1]=idx[1]*fData.res;
  cf.index[2]=idx[2]*fData.res;
  if(this->width<=3){value=getCenterValue(node);}
  else{
    TreeOctNode::ProcessPointAdjacentNodes(fData.depth,idx,&tree,width,&cf);
    value=cf.value;
  }
  if(value<isoValue){for(i=0;i<Cube::CORNERS;i++){cornerIndex2[i]|=1<<MarchingCubes::cornerMap[Cube::AntipodalCornerIndex(i)];}}
  // 9 of 27 set

  // Set face corners
  for(i=0;i<Cube::NEIGHBORS;i++){
    int dir,offset,e;
    Cube::FactorFaceIndex(i,dir,offset);
    cf.value=0;
    VertexData::FaceIndex(node,i,maxDepth,idx);
    cf.index[0]=idx[0]*fData.res;
    cf.index[1]=idx[1]*fData.res;
    cf.index[2]=idx[2]*fData.res;
    TreeOctNode::ProcessPointAdjacentNodes(fData.depth,idx,&tree,width,&cf);
    value=cf.value;
    Cube::FaceCorners(i,c[0],c[1],c[2],c[3]);
    e=Cube::EdgeIndex(dir,0,0);
    if(value<isoValue){for(j=0;j<4;j++){cornerIndex2[c[j]]|=1<<MarchingCubes::cornerMap[Cube::EdgeReflectCornerIndex(c[j],e)];}}
  }
  // 15 of 27 set

  // Set edge corners
  for(i=0;i<Cube::EDGES;i++){
    int o,i1,i2,f;
    Cube::FactorEdgeIndex(i,o,i1,i2);
    cf.value=0;
    VertexData::EdgeIndex(node,i,maxDepth,idx);
    cf.index[0]=idx[0]*fData.res;
    cf.index[1]=idx[1]*fData.res;
    cf.index[2]=idx[2]*fData.res;
    TreeOctNode::ProcessPointAdjacentNodes(fData.depth,idx,&tree,width,&cf);
    value=cf.value;
    Cube::EdgeCorners(i,c[0],c[1]);
    f=Cube::FaceIndex(o,0);
    if(value<isoValue){for(j=0;j<2;j++){cornerIndex2[c[j]]|=1<<MarchingCubes::cornerMap[Cube::FaceReflectCornerIndex(c[j],f)];}}
  }
  // 27 of 27 set

  for(i=0;i<Cube::CORNERS;i++){node->children[i].nodeData.mcIndex=cornerIndex2[i];}
}

template<int Degree>
inline int Octree<Degree>::InteriorFaceRootCount(const TreeOctNode* node,const int &faceIndex,const int& maxDepth)
{
  int c1,c2,e1,e2,dir,off,cnt=0;
  int corners[Cube::CORNERS/2];
  if(node->children){
    Cube::FaceCorners(faceIndex,corners[0],corners[1],corners[2],corners[3]);
    Cube::FactorFaceIndex(faceIndex,dir,off);
    c1=corners[0];
    c2=corners[3];
    switch(dir){
    case 0:
      e1=Cube::EdgeIndex(1,off,1);
      e2=Cube::EdgeIndex(2,off,1);
      break;
    case 1:
      e1=Cube::EdgeIndex(0,off,1);
      e2=Cube::EdgeIndex(2,1,off);
      break;
    case 2:
      e1=Cube::EdgeIndex(0,1,off);
      e2=Cube::EdgeIndex(1,1,off);
      break;
    };
    cnt+=EdgeRootCount(&node->children[c1],e1,maxDepth)+EdgeRootCount(&node->children[c1],e2,maxDepth);
    switch(dir){
    case 0:
      e1=Cube::EdgeIndex(1,off,0);
      e2=Cube::EdgeIndex(2,off,0);
      break;
    case 1:
      e1=Cube::EdgeIndex(0,off,0);
      e2=Cube::EdgeIndex(2,0,off);
      break;
    case 2:
      e1=Cube::EdgeIndex(0,0,off);
      e2=Cube::EdgeIndex(1,0,off);
      break;
    };
    cnt+=EdgeRootCount(&node->children[c2],e1,maxDepth)+EdgeRootCount(&node->children[c2],e2,maxDepth);
    for(int i=0;i<Cube::CORNERS/2;i++){if(node->children[corners[i]].children){cnt+=InteriorFaceRootCount(&node->children[corners[i]],faceIndex,maxDepth);}}
  }
  return cnt;
}

template<int Degree>
inline int Octree<Degree>::EdgeRootCount(const TreeOctNode* node,const int& edgeIndex,const int& maxDepth)
{
  int f1,f2,c1,c2;
  const TreeOctNode* temp;
  Cube::FacesAdjacentToEdge(edgeIndex,f1,f2);

  int eIndex;
  const TreeOctNode* finest=node;
  eIndex=edgeIndex;
  if(node->depth()<maxDepth){
    temp=node->faceNeighbor(f1);
    if(temp && temp->children){
      finest=temp;
      eIndex=Cube::FaceReflectEdgeIndex(edgeIndex,f1);
    }
    else{
      temp=node->faceNeighbor(f2);
      if(temp && temp->children){
        finest=temp;
        eIndex=Cube::FaceReflectEdgeIndex(edgeIndex,f2);
      }
      else{
        temp=node->edgeNeighbor(edgeIndex);
        if(temp && temp->children){
          finest=temp;
          eIndex=Cube::EdgeReflectEdgeIndex(edgeIndex);
        }
      }
    }
  }

  Cube::EdgeCorners(eIndex,c1,c2);
  if(finest->children){return EdgeRootCount(&finest->children[c1],eIndex,maxDepth)+EdgeRootCount(&finest->children[c2],eIndex,maxDepth);}
  else{return MarchingCubes::HasEdgeRoots(finest->nodeData.mcIndex,eIndex);}
}

template<int Degree>
inline int Octree<Degree>::IsBoundaryFace(const TreeOctNode* node,const int& faceIndex,const int& subdivideDepth)
{
  int dir,offset,d,o[3],idx;

  if(subdivideDepth<0){return 0;}
  if(node->d<=subdivideDepth){return 1;}
  Cube::FactorFaceIndex(faceIndex,dir,offset);
  node->depthAndOffset(d,o);

  idx=(int(o[dir])<<1) + (offset<<1);
  return !(idx%(2<<(int(node->d)-subdivideDepth)));
}

template<int Degree>
inline int Octree<Degree>::IsBoundaryEdge(const TreeOctNode* node,const int& edgeIndex,const int& subdivideDepth)
{
  int dir,x,y;
  Cube::FactorEdgeIndex(edgeIndex,dir,x,y);
  return IsBoundaryEdge(node,dir,x,y,subdivideDepth);
}

template<int Degree>
inline int Octree<Degree>::IsBoundaryEdge(const TreeOctNode* node,const int& dir,const int& x,const int& y,const int& subdivideDepth)
{
  int d=0,o[3],idx1=0,idx2=0,mask=0;

  if(subdivideDepth<0){return 0;}
  if(node->d<=subdivideDepth){return 1;}
  node->depthAndOffset(d,o);

  switch(dir){
  case 0:
    idx1=(int(o[1])<<1) + (x<<1);
    idx2=(int(o[2])<<1) + (y<<1);
    break;
  case 1:
    idx1=(int(o[0])<<1) + (x<<1);
    idx2=(int(o[2])<<1) + (y<<1);
    break;
  case 2:
    idx1=(int(o[0])<<1) + (x<<1);
    idx2=(int(o[1])<<1) + (y<<1);
    break;
  }
  mask=2<<(int(node->d)-subdivideDepth);
  return !(idx1%(mask)) || !(idx2%(mask));
}

template<int Degree>
inline void Octree<Degree>::PreValidate(TreeOctNode* node,const Real& isoValue,const int& maxDepth,const int& subdivideDepth)
{
  int sub=0;
  if(node->children){printf("Bad Pre-Validate\n");}
//	if(int(node->d)<subdivideDepth){sub=1;}
  for(int i=0;i<Cube::NEIGHBORS && !sub;i++){
    TreeOctNode* neighbor=node->faceNeighbor(i);
    if(neighbor && neighbor->children){
      if(IsBoundaryFace(node,i,subdivideDepth) && InteriorFaceRootCount(neighbor,Cube::FaceReflectFaceIndex(i,i),maxDepth)){sub=1;}
    }
  }
  if(sub){
    Subdivide(node,isoValue,maxDepth);
    for(int i=0;i<Cube::NEIGHBORS;i++){
      if(IsBoundaryFace(node,i,subdivideDepth) && InteriorFaceRootCount(node,i,maxDepth)){
        TreeOctNode* neighbor=node->faceNeighbor(i);
        while(neighbor && !neighbor->children){
          PreValidate(neighbor,isoValue,maxDepth,subdivideDepth);
          neighbor=node->faceNeighbor(i);
        }
      }
    }
  }
}

template<int Degree>
inline void Octree<Degree>::PreValidate(const Real& isoValue,const int& maxDepth,const int& subdivideDepth)
{
  TreeOctNode* temp;

  temp=tree.nextLeaf();
  while(temp){
    PreValidate(temp,isoValue,maxDepth,subdivideDepth);
    temp=tree.nextLeaf(temp);
  }
}

template<int Degree>
inline void Octree<Degree>::Validate(TreeOctNode* node,const Real& isoValue,const int& maxDepth,const int& fullDepthIso)
{
  int i,sub=0;
  TreeOctNode* treeNode=node;
  TreeOctNode* neighbor;
  if(node->depth()>=maxDepth || node->children){return;}

  // Check if full-depth extraction is enabled and we have an iso-node that is not at maximum depth
  if(!sub && fullDepthIso && MarchingCubes::HasRoots(node->nodeData.mcIndex)){sub=1;}

  // Check if the node has faces that are ambiguous and are adjacent to finer neighbors
  for(i=0;i<Cube::NEIGHBORS && !sub;i++){
    neighbor=treeNode->faceNeighbor(i);
    if(neighbor && neighbor->children){if(MarchingCubes::IsAmbiguous(node->nodeData.mcIndex,i)){sub=1;}}
  }

  // Check if the node has edges with more than one root
  for(i=0;i<Cube::EDGES && !sub;i++){if(EdgeRootCount(node,i,maxDepth)>1){sub=1;}}

  for(i=0;i<Cube::NEIGHBORS && !sub;i++){
    neighbor=node->faceNeighbor(i);
    if(	neighbor && neighbor->children &&
        !MarchingCubes::HasFaceRoots(node->nodeData.mcIndex,i) &&
        InteriorFaceRootCount(neighbor,Cube::FaceReflectFaceIndex(i,i),maxDepth)){sub=1;}
  }
  if(sub){
    Subdivide(node,isoValue,maxDepth);
    for(i=0;i<Cube::NEIGHBORS;i++){
      neighbor=treeNode->faceNeighbor(i);
      if(neighbor && !neighbor->children){Validate(neighbor,isoValue,maxDepth,fullDepthIso);}
    }
    for(i=0;i<Cube::EDGES;i++){
      neighbor=treeNode->edgeNeighbor(i);
      if(neighbor && !neighbor->children){Validate(neighbor,isoValue,maxDepth,fullDepthIso);}
    }
    for(i=0;i<Cube::CORNERS;i++){if(!node->children[i].children){Validate(&node->children[i],isoValue,maxDepth,fullDepthIso);}}
  }
}

template<int Degree>
inline void Octree<Degree>::Validate(TreeOctNode* node,const Real& isoValue,const int& maxDepth,const int& fullDepthIso,const int& subdivideDepth)
{
  int i,sub=0;
  TreeOctNode* treeNode=node;
  TreeOctNode* neighbor;
  if(node->depth()>=maxDepth || node->children){return;}

  // Check if full-depth extraction is enabled and we have an iso-node that is not at maximum depth
  if(!sub && fullDepthIso && MarchingCubes::HasRoots(node->nodeData.mcIndex)){sub=1;}

  // Check if the node has faces that are ambiguous and are adjacent to finer neighbors
  for(i=0;i<Cube::NEIGHBORS && !sub;i++){
    neighbor=treeNode->faceNeighbor(i);
    if(neighbor && neighbor->children){if(MarchingCubes::IsAmbiguous(node->nodeData.mcIndex,i) || IsBoundaryFace(node,i,subdivideDepth)){sub=1;}}
  }

  // Check if the node has edges with more than one root
  for(i=0;i<Cube::EDGES && !sub;i++){if(EdgeRootCount(node,i,maxDepth)>1){sub=1;}}

  for(i=0;i<Cube::NEIGHBORS && !sub;i++){
    neighbor=node->faceNeighbor(i);
    if(	neighbor && neighbor->children && !MarchingCubes::HasFaceRoots(node->nodeData.mcIndex,i) &&
        InteriorFaceRootCount(neighbor,Cube::FaceReflectFaceIndex(i,i),maxDepth)){sub=1;}
  }
  if(sub){
    Subdivide(node,isoValue,maxDepth);
    for(i=0;i<Cube::NEIGHBORS;i++){
      neighbor=treeNode->faceNeighbor(i);
      if(neighbor && !neighbor->children){Validate(neighbor,isoValue,maxDepth,fullDepthIso,subdivideDepth);}
    }
    for(i=0;i<Cube::EDGES;i++){
      neighbor=treeNode->edgeNeighbor(i);
      if(neighbor && !neighbor->children){Validate(neighbor,isoValue,maxDepth,fullDepthIso,subdivideDepth);}
    }
    for(i=0;i<Cube::CORNERS;i++){if(!node->children[i].children){Validate(&node->children[i],isoValue,maxDepth,fullDepthIso,subdivideDepth);}}
  }
}
//////////////////////////////////////////////////////////////////////////////////////
// The assumption made when calling this code is that the edge has at most one root //
//////////////////////////////////////////////////////////////////////////////////////
template<int Degree>
inline int Octree<Degree>::GetRoot(const RootInfo& ri,const Real& isoValue,Point3D<Real> & position,std::unordered_map<long long,std::pair<Real,Point3D<Real> > >& normalHash,const int& nonLinearFit)
{
  int c1,c2;
  Cube::EdgeCorners(ri.edgeIndex,c1,c2);
  if(!MarchingCubes::HasEdgeRoots(ri.node->nodeData.mcIndex,ri.edgeIndex)){return 0;}

  long long key;
  Point3D<Real> n[2];
  PointIndexValueAndNormalFunction cnf;
  cnf.valueTables=fData.valueTables;
  cnf.dValueTables=fData.dValueTables;
  cnf.res2=fData.res2;

  int i,o,i1,i2,rCount=0;
  Polynomial<2> P;
  std::vector<double> roots;
  double x0,x1;
  Real center,twidth;
  Real averageRoot=0;
  Cube::FactorEdgeIndex(ri.edgeIndex,o,i1,i2);
  int idx[3];
  key=VertexData::CornerIndex(ri.node,c1,fData.depth,idx);
  cnf.index[0]=idx[0]*fData.res;
  cnf.index[1]=idx[1]*fData.res;
  cnf.index[2]=idx[2]*fData.res;

  if(normalHash.find(key)==normalHash.end()){
    cnf.value=0;
    cnf.normal.coords[0]=cnf.normal.coords[1]=cnf.normal.coords[2]=0;
    // Careful here as the normal isn't quite accurate... (i.e. postNormalSmooth is ignored)
    if(this->width<=3){getCornerValueAndNormal(ri.node,c1,cnf.value,cnf.normal);}
    else{TreeOctNode::ProcessPointAdjacentNodes(fData.depth,idx,&tree,this->width,&cnf);}
    normalHash[key]=std::pair<Real,Point3D<Real> >(cnf.value,cnf.normal);
  }
  x0=normalHash[key].first;
  n[0]=normalHash[key].second;

  key=VertexData::CornerIndex(ri.node,c2,fData.depth,idx);
  cnf.index[0]=idx[0]*fData.res;
  cnf.index[1]=idx[1]*fData.res;
  cnf.index[2]=idx[2]*fData.res;
  if(normalHash.find(key)==normalHash.end()){
    cnf.value=0;
    cnf.normal.coords[0]=cnf.normal.coords[1]=cnf.normal.coords[2]=0;
    if(this->width<=3){getCornerValueAndNormal(ri.node,c2,cnf.value,cnf.normal);}
    else{TreeOctNode::ProcessPointAdjacentNodes(fData.depth,idx,&tree,this->width,&cnf);}
    normalHash[key]=std::pair<Real,Point3D<Real> >(cnf.value,cnf.normal);
  }
  x1=normalHash[key].first;
  n[1]=normalHash[key].second;

  Point3D<Real> c;
  ri.node->centerAndWidth(c,twidth);
  center=c.coords[o];
  for(i=0;i<DIMENSION;i++){
    n[0].coords[i]*=twidth;
    n[1].coords[i]*=twidth;
  }

  switch(o){
  case 0:
    position.coords[1]=c.coords[1]-twidth/2+twidth*i1;
    position.coords[2]=c.coords[2]-twidth/2+twidth*i2;
    break;
  case 1:
    position.coords[0]=c.coords[0]-twidth/2+twidth*i1;
    position.coords[2]=c.coords[2]-twidth/2+twidth*i2;
    break;
  case 2:
    position.coords[0]=c.coords[0]-twidth/2+twidth*i1;
    position.coords[1]=c.coords[1]-twidth/2+twidth*i2;
    break;
  }
  double dx0,dx1;
  dx0=n[0].coords[o];
  dx1=n[1].coords[o];

  // The scaling will turn the Hermite Spline into a quadratic
  double scl=(x1-x0)/((dx1+dx0)/2);
  dx0*=scl;
  dx1*=scl;

  // Hermite Spline
  P.coefficients[0]=x0;
  P.coefficients[1]=dx0;
  P.coefficients[2]=3*(x1-x0)-dx1-2*dx0;

  P.getSolutions(isoValue,roots,EPSILON);
  for(i=0;i<int(roots.size());i++){
    if(roots[i]>=0 && roots[i]<=1){
      averageRoot+=Real(roots[i]);
      rCount++;
    }
  }
  if(rCount && nonLinearFit)	{averageRoot/=rCount;}
  else     {averageRoot=Real((x0-isoValue)/(x0-x1));
    if (averageRoot > 1) averageRoot=1;
    if (averageRoot < -1) averageRoot=-1;
  }

  position.coords[o]=Real(center-twidth/2+twidth*averageRoot);
  return 1;
}

template<int Degree>
inline int Octree<Degree>::GetRoot(const RootInfo& ri,const Real& isoValue,const int& maxDepth,Point3D<Real>& position,std::unordered_map<long long,std::pair<Real,Point3D<Real> > >& Normals,
                                   Point3D<Real>* normal,const int& nonLinearFit)
{
  (void) maxDepth;
  (void) normal;

  if(!MarchingCubes::HasRoots(ri.node->nodeData.mcIndex)){return 0;}
  return GetRoot(ri,isoValue,position,Normals,nonLinearFit);
}

template<int Degree>
inline int Octree<Degree>::GetRootIndex(const TreeOctNode* node,const int& edgeIndex,const int& maxDepth,const int& sDepth,RootInfo& ri)
{
  int c1,c2,f1,f2;
  const TreeOctNode *temp,*finest;
  int finestIndex;

  Cube::FacesAdjacentToEdge(edgeIndex,f1,f2);

  finest=node;
  finestIndex=edgeIndex;
  if(node->depth()<maxDepth){
    if(IsBoundaryFace(node,f1,sDepth)){temp=NULL;}
    else{temp=node->faceNeighbor(f1);}
    if(temp && temp->children){
      finest=temp;
      finestIndex=Cube::FaceReflectEdgeIndex(edgeIndex,f1);
    }
    else{
      if(IsBoundaryFace(node,f2,sDepth)){temp=NULL;}
      else{temp=node->faceNeighbor(f2);}
      if(temp && temp->children){
        finest=temp;
        finestIndex=Cube::FaceReflectEdgeIndex(edgeIndex,f2);
      }
      else{
        if(IsBoundaryEdge(node,edgeIndex,sDepth)){temp=NULL;}
        else{temp=node->edgeNeighbor(edgeIndex);}
        if(temp && temp->children){
          finest=temp;
          finestIndex=Cube::EdgeReflectEdgeIndex(edgeIndex);
        }
      }
    }
  }

  Cube::EdgeCorners(finestIndex,c1,c2);
  if(finest->children){
    if		(GetRootIndex(&finest->children[c1],finestIndex,maxDepth,sDepth,ri))	{return 1;}
    else if	(GetRootIndex(&finest->children[c2],finestIndex,maxDepth,sDepth,ri))	{return 1;}
    else																							{return 0;}
  }
  else{
    if(!(MarchingCubes::edgeMask[finest->nodeData.mcIndex] & (1<<finestIndex))){return 0;}

    int o,i1,i2;
    Cube::FactorEdgeIndex(finestIndex,o,i1,i2);
    int d,off[3];
    finest->depthAndOffset(d,off);
    ri.node=finest;
    ri.edgeIndex=finestIndex;
    int eIndex[2],offset;
    offset=BinaryNode<Real>::Index(d,off[o]);
    switch(o){
    case 0:
      eIndex[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[1],i1);
      eIndex[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[2],i2);
      break;
    case 1:
      eIndex[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[0],i1);
      eIndex[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[2],i2);
      break;
    case 2:
      eIndex[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[0],i1);
      eIndex[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[1],i2);
      break;
    }
    ri.key = (long long)(o) | (long long)(eIndex[0])<<5 | (long long)(eIndex[1])<<25 | (long long)(offset)<<45;
    return 1;
  }
}

template<int Degree>
inline int Octree<Degree>::GetRootIndex(const TreeOctNode* node,const int& edgeIndex,const int& maxDepth,RootInfo& ri)
{
  int c1,c2,f1,f2;
  const TreeOctNode *temp,*finest;
  int finestIndex;


  // The assumption is that the super-edge has a root along it.
  if(!(MarchingCubes::edgeMask[node->nodeData.mcIndex] & (1<<edgeIndex))){return 0;}

  Cube::FacesAdjacentToEdge(edgeIndex,f1,f2);

  finest=node;
  finestIndex=edgeIndex;
  if(node->depth()<maxDepth){
    temp=node->faceNeighbor(f1);
    if(temp && temp->children){
      finest=temp;
      finestIndex=Cube::FaceReflectEdgeIndex(edgeIndex,f1);
    }
    else{
      temp=node->faceNeighbor(f2);
      if(temp && temp->children){
        finest=temp;
        finestIndex=Cube::FaceReflectEdgeIndex(edgeIndex,f2);
      }
      else{
        temp=node->edgeNeighbor(edgeIndex);
        if(temp && temp->children){
          finest=temp;
          finestIndex=Cube::EdgeReflectEdgeIndex(edgeIndex);
        }
      }
    }
  }

  Cube::EdgeCorners(finestIndex,c1,c2);
  if(finest->children){
    if		(GetRootIndex(&finest->children[c1],finestIndex,maxDepth,ri))				{return 1;}
    else if	(GetRootIndex(&finest->children[c2],finestIndex,maxDepth,ri))				{return 1;}
    else																				{return 0;}
  }
  else{
    int o,i1,i2;
    Cube::FactorEdgeIndex(finestIndex,o,i1,i2);
    int d,off[3];
    finest->depthAndOffset(d,off);
    ri.node=finest;
    ri.edgeIndex=finestIndex;
    int offset,eIndex[2];
    eIndex[0] = 0;
    eIndex[1] = 0;
    offset=BinaryNode<Real>::Index(d,off[o]);
    switch(o){
    case 0:
      eIndex[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[1],i1);
      eIndex[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[2],i2);
      break;
    case 1:
      eIndex[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[0],i1);
      eIndex[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[2],i2);
      break;
    case 2:
      eIndex[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[0],i1);
      eIndex[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[1],i2);
      break;
    }
    ri.key= (long long)(o) | (long long)(eIndex[0])<<5 | (long long)(eIndex[1])<<25 | (long long)(offset)<<45;
    return 1;
  }
}
template<int Degree>
inline int Octree<Degree>::GetRootPair(const RootInfo& ri,const int& maxDepth,RootInfo& pair)
{
  const TreeOctNode* node=ri.node;
  int c1,c2,c;
  Cube::EdgeCorners(ri.edgeIndex,c1,c2);
  while(node->parent){
    c=int(node-node->parent->children);
    if(c!=c1 && c!=c2){return 0;}
    if(!MarchingCubes::HasEdgeRoots(node->parent->nodeData.mcIndex,ri.edgeIndex)){
      if(c==c1){return GetRootIndex(&node->parent->children[c2],ri.edgeIndex,maxDepth,pair);}
      else{return GetRootIndex(&node->parent->children[c1],ri.edgeIndex,maxDepth,pair);}
    }
    node=node->parent;
  }
  return 0;

}

template<int Degree>
inline int Octree<Degree>::GetRootIndex(const long long& key,std::unordered_map<long long,int>& boundaryRoots,std::unordered_map<long long,int>* interiorRoots,CoredPointIndex& index)
{
  std::unordered_map<long long,int>::iterator rootIter=boundaryRoots.find(key);
  if(rootIter!=boundaryRoots.end()){
    index.inCore=1;
    index.index=rootIter->second;
    return 1;
  }
  else if(interiorRoots){
    rootIter=interiorRoots->find(key);
    if(rootIter!=interiorRoots->end()){
      index.inCore=0;
      index.index=rootIter->second;
      return 1;
    }
  }
  return 0;
}

template<int Degree>
inline int Octree<Degree>::SetMCRootPositions(TreeOctNode* node,const int& sDepth,const Real& isoValue,
                                              std::unordered_map<long long,int>& boundaryRoots,std::unordered_map<long long,int>* interiorRoots,
                                              std::unordered_map<long long,std::pair<Real,Point3D<Real> > >& boundaryNormalHash,std::unordered_map<long long,std::pair<Real,Point3D<Real> > >* interiorNormalHash,
                                              std::vector<Point3D<float> >* interiorPositions,
                                              CoredMeshData* mesh,const int& nonLinearFit)
{
  Point3D<Real> position;
  int i,j,k,eIndex;
  RootInfo ri;
  int count=0;
  if(!MarchingCubes::HasRoots(node->nodeData.mcIndex)){return 0;}
  for(i=0;i<DIMENSION;i++){
    for(j=0;j<2;j++){
      for(k=0;k<2;k++){
        long long key;
        eIndex=Cube::EdgeIndex(i,j,k);
        if(GetRootIndex(node,eIndex,fData.depth,ri)){
          key=ri.key;
          if(!interiorRoots || IsBoundaryEdge(node,i,j,k,sDepth)){
            if(boundaryRoots.find(key)==boundaryRoots.end()){
              GetRoot(ri,isoValue,fData.depth,position,boundaryNormalHash,NULL,nonLinearFit);
              mesh->inCorePoints.push_back(position);
              boundaryRoots[key]=int(mesh->inCorePoints.size())-1;
              count++;
            }
          }
          else{
            if(interiorRoots->find(key)==interiorRoots->end()){
              GetRoot(ri,isoValue,fData.depth,position,*interiorNormalHash,NULL,nonLinearFit);
              (*interiorRoots)[key]=mesh->addOutOfCorePoint(position);
              interiorPositions->push_back(position);
              count++;
            }
          }
        }
      }
    }
  }
  return count;
}

template<int Degree>
inline int Octree<Degree>::SetBoundaryMCRootPositions(const int& sDepth,const Real& isoValue,
                                                      std::unordered_map<long long,int>& boundaryRoots,std::unordered_map<long long,std::pair<Real,Point3D<Real> > >& boundaryNormalHash,
                                                      CoredMeshData* mesh,const int& nonLinearFit)
{
  Point3D<Real> position;
  int i,j,k,eIndex,hits=0;
  RootInfo ri;
  int count=0;
  TreeOctNode* node;

  node=tree.nextLeaf();
  while(node){
    if(MarchingCubes::HasRoots(node->nodeData.mcIndex)){
      hits=0;
      for(i=0;i<DIMENSION;i++){
        for(j=0;j<2;j++){
          for(k=0;k<2;k++){
            if(IsBoundaryEdge(node,i,j,k,sDepth)){
              hits++;
              long long key;
              eIndex=Cube::EdgeIndex(i,j,k);
              if(GetRootIndex(node,eIndex,fData.depth,ri)){
                key=ri.key;
                if(boundaryRoots.find(key)==boundaryRoots.end()){
                  GetRoot(ri,isoValue,fData.depth,position,boundaryNormalHash,NULL,nonLinearFit);
                  mesh->inCorePoints.push_back(position);
                  boundaryRoots[key]=int(mesh->inCorePoints.size())-1;
                  count++;
                }
              }
            }
          }
        }
      }
    }
    if(hits){node=tree.nextLeaf(node);}
    else{node=tree.nextBranch(node);}
  }
  return count;
}

template<int Degree>
inline void Octree<Degree>::GetMCIsoEdges(TreeOctNode* node,std::unordered_map<long long,int>& boundaryRoots,std::unordered_map<long long,int>* interiorRoots,const int& sDepth,
                                          std::vector<std::pair<long long,long long> >& edges)
{
  (void) boundaryRoots;
  (void) interiorRoots;

  TreeOctNode* temp;
  int count=0;
  int isoTri[DIMENSION*MarchingCubes::MAX_TRIANGLES];
  FaceEdgesFunction fef;
  int ref,fIndex;
  std::unordered_map<long long,std::pair<RootInfo,int> >::iterator iter;
  std::unordered_map<long long,std::pair<RootInfo,int> > vertexCount;

  fef.edges=&edges;
  fef.maxDepth=fData.depth;
  fef.vertexCount=&vertexCount;
  count=MarchingCubes::AddTriangleIndices(node->nodeData.mcIndex,isoTri);
  for(fIndex=0;fIndex<Cube::NEIGHBORS;fIndex++){
    ref=Cube::FaceReflectFaceIndex(fIndex,fIndex);
    fef.fIndex=ref;
    temp=node->faceNeighbor(fIndex);
    // If the face neighbor exists and has higher resolution than the current node,
    // get the iso-curve from the neighbor
    if(temp && temp->children && !IsBoundaryFace(node,fIndex,sDepth)){temp->processNodeFaces(temp,&fef,ref);}
    // Otherwise, get it from the node
    else{
      RootInfo ri1,ri2;
      for(int j=0;j<count;j++){
        for(int k=0;k<3;k++){
          if(fIndex==Cube::FaceAdjacentToEdges(isoTri[j*3+k],isoTri[j*3+((k+1)%3)])){
            if(GetRootIndex(node,isoTri[j*3+k],fData.depth,ri1) && GetRootIndex(node,isoTri[j*3+((k+1)%3)],fData.depth,ri2)){
              edges.push_back(std::pair<long long,long long>(ri1.key,ri2.key));
              iter=vertexCount.find(ri1.key);
              if(iter==vertexCount.end()){
                vertexCount[ri1.key].first=ri1;
                vertexCount[ri1.key].second=0;
              }
              iter=vertexCount.find(ri2.key);
              if(iter==vertexCount.end()){
                vertexCount[ri2.key].first=ri2;
                vertexCount[ri2.key].second=0;
              }
              vertexCount[ri1.key].second++;
              vertexCount[ri2.key].second--;
            }
            else{fprintf(stderr,"Bad Edge 1: %lld %lld\n",ri1.key,ri2.key);}
          }
        }
      }
    }
  }
  for(int i=0;i<int(edges.size());i++){
    iter=vertexCount.find(edges[i].first);
    if(iter==vertexCount.end()){printf("Could not find vertex: %lld\n",edges[i].first);}
    else if(vertexCount[edges[i].first].second){
      RootInfo ri;
      GetRootPair(vertexCount[edges[i].first].first,fData.depth,ri);
      iter=vertexCount.find(ri.key);
      if(iter==vertexCount.end()){printf("Vertex pair not in list\n");}
      else{
        edges.push_back(std::pair<long long,long long>(ri.key,edges[i].first));
        vertexCount[ri.key].second++;
        vertexCount[edges[i].first].second--;
      }
    }

    iter=vertexCount.find(edges[i].second);
    if(iter==vertexCount.end()){printf("Could not find vertex: %lld\n",edges[i].second);}
    else if(vertexCount[edges[i].second].second){
      RootInfo ri;
      GetRootPair(vertexCount[edges[i].second].first,fData.depth,ri);
      iter=vertexCount.find(ri.key);
      if(iter==vertexCount.end()){printf("Vertex pair not in list\n");}
      else{
        edges.push_back(std::pair<long long,long long>(edges[i].second,ri.key));
        vertexCount[edges[i].second].second++;
        vertexCount[ri.key].second--;
      }
    }
  }
}

template<int Degree>
inline int Octree<Degree>::GetMCIsoTriangles(TreeOctNode* node,CoredMeshData* mesh,std::unordered_map<long long,int>& boundaryRoots,
                                             std::unordered_map<long long,int>* interiorRoots,std::vector<Point3D<float> >* interiorPositions,const int& offSet,const int& sDepth, bool addBarycenter)
{
  int tris=0;
  std::vector<std::pair<long long,long long> > edges;
  std::vector<std::vector<std::pair<long long,long long> > > edgeLoops;
  GetMCIsoEdges(node,boundaryRoots,interiorRoots,sDepth,edges);

  GetEdgeLoops(edges,edgeLoops);
  for(int i=0;i<int(edgeLoops.size());i++){
    CoredPointIndex p;
    std::vector<CoredPointIndex> edgeIndices;
    for(int j=0;j<int(edgeLoops[i].size());j++){
      if(!GetRootIndex(edgeLoops[i][j].first,boundaryRoots,interiorRoots,p)){printf("Bad Point Index\n");}
      else{edgeIndices.push_back(p);}
    }
    tris+=AddTriangles(mesh,edgeIndices,interiorPositions,offSet,addBarycenter);
  }
  return tris;
}

template<int Degree>
inline int Octree<Degree>::GetEdgeLoops(std::vector<std::pair<long long,long long> >& edges,std::vector<std::vector<std::pair<long long,long long> > >& loops){
  int loopSize=0;
  long long frontIdx,backIdx;
  std::pair<long long,long long> e,temp;
  loops.clear();

  while(edges.size()){
    std::vector<std::pair<long long,long long> > front,back;
    e=edges[0];
    loops.resize(loopSize+1);
    edges[0]=edges[edges.size()-1];
    edges.pop_back();
    frontIdx=e.second;
    backIdx=e.first;
    for(int j=int(edges.size())-1;j>=0;j--){
      if(edges[j].first==frontIdx || edges[j].second==frontIdx){
        if(edges[j].first==frontIdx)	{temp=edges[j];}
        else							{temp.first=edges[j].second;temp.second=edges[j].first;}
        frontIdx=temp.second;
        front.push_back(temp);
        edges[j]=edges[edges.size()-1];
        edges.pop_back();
        j=int(edges.size());
      }
      else if(edges[j].first==backIdx || edges[j].second==backIdx){
        if(edges[j].second==backIdx)	{temp=edges[j];}
        else							{temp.first=edges[j].second;temp.second=edges[j].first;}
        backIdx=temp.first;
        back.push_back(temp);
        edges[j]=edges[edges.size()-1];
        edges.pop_back();
        j=int(edges.size());
      }
    }
    for(int j=int(back.size())-1;j>=0;j--){loops[loopSize].push_back(back[j]);}
    loops[loopSize].push_back(e);
    for(int j=0;j<int(front.size());j++){loops[loopSize].push_back(front[j]);}
    loopSize++;
  }
  return int(loops.size());
}

template<int Degree>
inline int Octree<Degree>::AddTriangles(CoredMeshData* mesh,std::vector<CoredPointIndex> edges[3],std::vector<Point3D<float> >* interiorPositions,const int& offSet)
{
  std::vector<CoredPointIndex> e[3];
  for(int i=0;i<3;i++){for(size_t j=0;j<edges[i].size();j++){e[i].push_back(edges[i][j]);}}
  return AddTriangles(mesh,e,interiorPositions,offSet);
}

template<int Degree>
int Octree<Degree>::AddTriangles( CoredMeshData* mesh , std::vector<CoredPointIndex>& edges , std::vector<Point3D<float> >* interiorPositions , const int& offSet , bool addBarycenter )
{
  if( edges.size()>3 )
  {
    bool isCoplanar = false;

    for( size_t i=0 ; i<edges.size() ; i++ )
      for( size_t j=0 ; j<i ; j++ )
        if( (i+1)%edges.size()!=j && (j+1)%edges.size()!=i )
        {
          Point3D< Real > v1 , v2;
          if( edges[i].inCore ) for( int k=0 ; k<3 ; k++ ) v1.coords[k] = mesh->inCorePoints[ edges[i].index ].coords[k];
          else for( int k=0 ; k<3 ; k++ ) v1.coords[k] = (*interiorPositions)[ edges[i].index-offSet ].coords[k];
          if( edges[j].inCore ) for( int k=0 ; k<3 ; k++ ) v2.coords[k] = mesh->inCorePoints[ edges[j].index ].coords[k];
          else for( int k=0 ; k<3 ; k++ ) v2.coords[k] = (*interiorPositions)[ edges[j].index-offSet ].coords[k];
          for( int k=0 ; k<3 ; k++ ) if( v1.coords[k]==v2.coords[k] ) isCoplanar = true;
        }
    if( addBarycenter && isCoplanar )
    {
      Point3D< Real > c;
      c.coords[0] = c.coords[1] = c.coords[2] = 0;
      for( int i=0 ; i<int(edges.size()) ; i++ )
      {
        Point3D<Real> p;
        if(edges[i].inCore) for(int j=0 ; j<3 ; j++ ) p.coords[j] = mesh->inCorePoints[edges[i].index].coords[j];
        else for(int j=0 ; j<3 ; j++ ) p.coords[j] =(*interiorPositions)[edges[i].index-offSet].coords[j];
        c.coords[0] += p.coords[0] , c.coords[1] += p.coords[1] , c.coords[2] += p.coords[2];
      }
      c.coords[0] /= edges.size() , c.coords[1] /= edges.size() , c.coords[2] /= edges.size();
      int cIdx = mesh->addOutOfCorePoint( c );
      for( int i=0 ; i<int(edges.size()) ; i++ )
      {
        int inCoreFlag=0;
        TriangleIndex tri;
        tri.idx[0] = edges[i].index;
        tri.idx[1] = edges[(i+1)%edges.size()].index;
        tri.idx[2] = cIdx;
        if( edges[i ].inCore ) inCoreFlag |= CoredMeshData::IN_CORE_FLAG[0];
        if( edges[(i+1)%edges.size()].inCore ) inCoreFlag |= CoredMeshData::IN_CORE_FLAG[1];
        mesh->addTriangle( tri , inCoreFlag );
      }
      return edges.size();
    }
    else
    {
      Triangulation<float> t;

      // Add the points to the triangulation
      for(int i=0;i<int(edges.size());i++)
      {
        Point3D<Real> p;
        if(edges[i].inCore) {for(int j=0;j<3;j++){p.coords[j]=mesh->inCorePoints[edges[i].index].coords[j];}}
        else {for(int j=0;j<3;j++){p.coords[j]=(*interiorPositions)[edges[i].index-offSet].coords[j];}}
        t.points.push_back(p);
      }

      // Create a fan triangulation
      for(int i=1;i<int(edges.size())-1;i++){t.addTriangle(0,i,i+1);}

      // Minimize
      while(1){
        unsigned int i;
        for(i=0;i<t.edges.size();i++){if(t.flipMinimize(i)){break;}}
        if(i==t.edges.size()){break;}
      }
      // Add the triangles to the mesh
      for(int i=0;i<int(t.triangles.size());i++){
        TriangleIndex tri;
        int idx[3];
        idx[0] = 0;
        idx[1] = 0;
        idx[2] = 0;
        int inCoreFlag=0;
        t.factor(i,idx[0],idx[1],idx[2]);
        for(int j=0;j<3;j++){
          tri.idx[j]=edges[idx[j]].index;
          if(edges[idx[j]].inCore){inCoreFlag|=CoredMeshData::IN_CORE_FLAG[j];}
        }
        mesh->addTriangle(tri,inCoreFlag);
      }
    }
  }
  else if(edges.size()==3){
    TriangleIndex tri;
    int inCoreFlag=0;
    for(int i=0;i<3;i++){
      tri.idx[i]=edges[i].index;
      if(edges[i].inCore){inCoreFlag|=CoredMeshData::IN_CORE_FLAG[i];}
    }
    mesh->addTriangle(tri,inCoreFlag);
  }
  return int(edges.size())-2;
}
////////////////
// VertexData //
////////////////
inline long long VertexData::CenterIndex(const TreeOctNode* node,const int& maxDepth)
{
  int idx[DIMENSION];
  return CenterIndex(node,maxDepth,idx);
}

inline long long VertexData::CenterIndex(const TreeOctNode* node,const int& maxDepth,int idx[DIMENSION])
{
  int d,o[3];
  node->depthAndOffset(d,o);
  for(int i=0;i<DIMENSION;i++){idx[i]=BinaryNode<Real>::CornerIndex(maxDepth+1,d+1,o[i]<<1,1);}
  return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30;
}

inline long long VertexData::CenterIndex(const int& depth,const int offSet[DIMENSION],const int& maxDepth,int idx[DIMENSION])
{
  for(int i=0;i<DIMENSION;i++){idx[i]=BinaryNode<Real>::CornerIndex(maxDepth+1,depth+1,offSet[i]<<1,1);}
  return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30;
}

inline long long VertexData::CornerIndex(const TreeOctNode* node,const int& cIndex,const int& maxDepth)
{
  int idx[DIMENSION];
  return CornerIndex(node,cIndex,maxDepth,idx);
}

inline long long VertexData::CornerIndex(const TreeOctNode* node,const int& cIndex,const int& maxDepth,int idx[DIMENSION])
{
  int x[DIMENSION];
  Cube::FactorCornerIndex(cIndex,x[0],x[1],x[2]);
  int d,o[3];
  node->depthAndOffset(d,o);
  for(int i=0;i<DIMENSION;i++){idx[i]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,o[i],x[i]);}
  return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30;
}

inline long long VertexData::CornerIndex(const int& depth,const int offSet[DIMENSION],const int& cIndex,const int& maxDepth,int idx[DIMENSION])
{
  int x[DIMENSION];
  Cube::FactorCornerIndex(cIndex,x[0],x[1],x[2]);
  for(int i=0;i<DIMENSION;i++){idx[i]=BinaryNode<Real>::CornerIndex(maxDepth+1,depth,offSet[i],x[i]);}
  return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30;
}

inline long long VertexData::FaceIndex(const TreeOctNode* node,const int& fIndex,const int& maxDepth)
{
  int idx[DIMENSION];
  return FaceIndex(node,fIndex,maxDepth,idx);
}

inline long long VertexData::FaceIndex(const TreeOctNode* node,const int& fIndex,const int& maxDepth,int idx[DIMENSION])
{
  int dir,offset;
  Cube::FactorFaceIndex(fIndex,dir,offset);
  int d,o[3];
  node->depthAndOffset(d,o);
  for(int i=0;i<DIMENSION;i++){idx[i]=BinaryNode<Real>::CornerIndex(maxDepth+1,d+1,o[i]<<1,1);}
  idx[dir]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,o[dir],offset);
  return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30;
}

inline long long VertexData::EdgeIndex(const TreeOctNode* node,const int& eIndex,const int& maxDepth)
{
  int idx[DIMENSION];
  return EdgeIndex(node,eIndex,maxDepth,idx);
}

inline long long VertexData::EdgeIndex(const TreeOctNode* node,const int& eIndex,const int& maxDepth,int idx[DIMENSION])
{
  int o,i1,i2;
  int d,off[3];
  node->depthAndOffset(d,off);
  for(int i=0;i<DIMENSION;i++){idx[i]=BinaryNode<Real>::CornerIndex(maxDepth+1,d+1,off[i]<<1,1);}
  Cube::FactorEdgeIndex(eIndex,o,i1,i2);
  switch(o){
  case 0:
    idx[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[1],i1);
    idx[2]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[2],i2);
    break;
  case 1:
    idx[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[0],i1);
    idx[2]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[2],i2);
    break;
  case 2:
    idx[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[0],i1);
    idx[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[1],i2);
    break;
  };
  return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30;
}
#if defined(_MSC_VER)
# pragma warning (pop)
#endif
